def draw_epipolar_lines(Irgb1, Irgb2, pts1, pts2, H):
""" Draws epipolar lines, and displays them to the user in an
interactive manner.
Input:
nparray Irgb1, Irgb2: H x W x 3
nparray pts1, pts2: N x 2
nparray H: 3x3
"""
h, w = Irgb1.shape[0:2]
for i, pt1 in enumerate(pts1):
Irgb1_ = Irgb1.copy()
Irgb2_ = Irgb2.copy()
pt2 = pts2[i]
pt1_h = np.hstack((pt1, np.array([1.0])))
pt2_h = np.hstack((pt2, np.array([1.0])))
epiline2 = np.dot(util_camera.make_crossprod_mat(pt2_h), np.dot(H, pt1_h))
epiline1 = np.dot(H.T, epiline2)
epiline1 = epiline1 / epiline1[2]
epiline2 = epiline2 / epiline2[2]
print "Epiline1 is: slope={0} y-int={1}".format(-epiline1[0] / epiline1[1],
-epiline1[2] / epiline1[1])
print "Epiline2 is: slope={0} y-int={1}".format(-epiline2[0] / epiline2[1],
-epiline2[2] / epiline2[1])
Irgb1_ = util_camera.draw_line(Irgb1_, epiline1)
Irgb2_ = util_camera.draw_line(Irgb2_, epiline2)
cv.Circle(cv.fromarray(Irgb1_), tuple(intrnd(*pts1[i])), 3, (255, 0, 0))
cv.Circle(cv.fromarray(Irgb2_), tuple(intrnd(*pts2[i])), 3, (255, 0, 0))
print "(Displaying epipolar lines from img1 to img2. Press <enter> to continue.)"
cv2.namedWindow('display1')
cv2.imshow('display1', Irgb1_)
cv2.namedWindow('display2')
cv2.imshow('display2', Irgb2_)
cv2.waitKey(0)
def paraToMat(para):
k_v = para[0:5]
K = array([[k_v[0], k_v[1], k_v[2]],
[0, k_v[3], k_v[4]],
[0, 0, 1]])
k1 = para[5]
k2 = para[6]
RTpara = para[7:]
RTpara = RTpara.reshape(len(RTpara)/6, 6)
#empty the original RT array
Array_RT = []
r_v = zeros((1, 3))
for idx in xrange(len(RTpara)):
para = RTpara[idx]
r_v[0] = copy(para[0:3])
R = zeros((3, 3))
cv.Rodrigues2(cv.fromarray(r_v), cv.fromarray(R))
t_v = para[3:6]
RT = zeros((3, 4))
RT[:, 0] = R[:, 0]
RT[:, 1] = R[:, 1]
RT[:, 2] = R[:, 2]
RT[:, 3] = t_v
Array_RT.append(RT)
return(K, Array_RT, k1, k2)
def LoadImage(filename, rotate180=False, RGB=False):
'''wrapper around cv.LoadImage that also handles PGM.
It always returns a colour image of the same size'''
if filename.endswith('.pgm'):
from ..image import scanner
try:
pgm = PGM(filename)
except Exception as e:
print('Failed to load %s: %s' % (filename, e))
return None
im_full = numpy.zeros((960,1280,3),dtype='uint8')
if RGB:
scanner.debayer_RGB(pgm.array, im_full)
else:
scanner.debayer(pgm.array, im_full)
if rotate180:
scanner.rotate180(im_full)
return cv.GetImage(cv.fromarray(im_full))
img = cv.LoadImage(filename)
if rotate180:
from ..image import scanner
img = numpy.ascontiguousarray(cv.GetMat(img))
scanner.rotate180(img)
img = cv.GetImage(cv.fromarray(img))
return img
def line_pro(self):
val,im = self.vid.read()
if val==True:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray,80,120)
cv2.imshow("image2",edges)
cv2.waitKey(5)
storage = cv.CreateMemStorage(0)
lines = cv.HoughLines2(cv.fromarray(edges), storage, cv.CV_HOUGH_STANDARD, 1, math.pi / 180, 120, 0,10)
for (rho, theta) in lines:
self.pro(rho,theta,lines,im)
a = math.cos(theta)
b = math.sin(theta)
x0 = a * rho
y0 = b * rho
pt1 = (cv.Round(x0 + 1000*(-b)), cv.Round(y0 + 1000*(a)))
pt2 = (cv.Round(x0 - 1000*(-b)), cv.Round(y0 - 1000*(a)))
cv.Line(cv.fromarray(im), pt1, pt2, cv.RGB(255, 0, 0), 3, 8)
cv2.imshow("image1",im)
cv2.waitKey(5)
#lines = cv.HoughLines2(cv.fromarray(edges), storage, cv.CV_HOUGH_PROBABILISTIC, 1,math.pi / 180,val, 50, 10)
#for lin in lines:
# print "lin=",lin
# theta=math.atan2((lin[1][1]-lin[0][1]),(lin[1][0]-lin[0][0]))
# if math.sin(theta)!=0:
# m=-(math.cos(theta)/math.sin(theta))
# c=lin[0][1]-(m*lin[0][0])
# self.pro(lines,m,c,lin,im)
# d=self.dist(lin)
# print "d=",d
# cv.Line(cv.fromarray(im), lin[0], lin[1], cv.CV_RGB(255, 0, 0), 3, 8)
# #print "lin[0]=",lin[0],"lin[1]=",lin[1]
cv2.imshow("image1",im)
cv2.waitKey(5)
def numpy_to_iplimage_color(numpy_format):
cvMat = cv.fromarray(numpy_format)
cv.SaveImage('cvMat.png', cvMat) #Saves the image
iplimage = cv.LoadImage('cvMat.png', cv.CV_LOAD_IMAGE_COLOR)
f**k = cv.fromarray(numpy_format)
os.system('rm cvMat.png')
return iplimage
def mouseDoubleClickEvent(self, event):
frame = self.bridge.imgmsg_to_cv(self.image, 'bgr8')
cv_image = np.array(frame, dtype=np.uint8)
unclosed_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
kernel = np.array([[1, 1, 1],[1, 1, 1],[1, 1, 1]], 'uint8')
kernel7 = np.ones((7,7),'uint8')
gray = cv2.dilate(cv2.erode(unclosed_gray, kernel7), kernel7)
edges = cv2.Canny(gray,50,150,apertureSize = 3)
# kernel = np.ones((3,3),'uint8')
dilated_edges = cv2.dilate(edges, kernel)
minLineLength = 140
maxLineGap = 10
lines = cv2.HoughLinesP(dilated_edges,1,np.pi/180,300,minLineLength,maxLineGap)
if lines is not None:
for x1,y1,x2,y2 in lines[0]:
cv2.line(cv_image,(x1,y1),(x2,y2),(0,255,0),2)
frame = cv.fromarray(cv_image)
edges = cv.fromarray(edges)
dilated_edges = cv.fromarray(dilated_edges)
cv.ShowImage("im window", frame)
cv.ShowImage("im window2", edges)
cv.ShowImage("im window3", dilated_edges)
cv.MoveWindow("im window2", 820, 60)
cv.MoveWindow("im window3", 820, 660)
print "Yoda"
def gotimage(self, imgmsg):
if imgmsg.encoding == "bayer_bggr8":
imgmsg.encoding = "8UC1"
img = CvBridge().imgmsg_to_cv(imgmsg)
# cv.ShowImage("DataMatrix", img)
# cv.WaitKey(6)
print "gotimage"
if(self.find == False):
return
self.track(img)
self.find = False
# monocular case
if 0 and 'l' in self.cams:
for (code, corners) in self.tracking.items():
model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(model,
cv.fromarray(numpy.array(corners, numpy.float32)),
self.cams['l'].intrinsicMatrix(),
self.cams['l'].distortionCoeffs(),
rot,
trans)
self.broadcast(imgmsg)
def follow(self,show):
key=0
framecount=0
offset=80
while(chr(key & 255)!='q' and self.stop!=True):
if self.frame!=None:
self.mutex.acquire()
frame=self.frame
self.mutex.release()
if chr(key & 255)=='d':
self.face_detected=0
if framecount%2==0:
faces=self.detect_faces(frame)
if faces!=():
self.calc_histogramm(faces[0],cv.fromarray(frame))
self.face_detected=1
center_point=(faces[0][0]+faces[0][2]/2,faces[0][1]+faces[0][3]/2)
if show:
for x,y,w,h in faces:
cv2.rectangle(numpy.asarray(frame), (x, y), (x+w, y+h), (0, 255, 0), 2)
cv2.circle(numpy.asarray(frame), tuple(map(int,center_point)), 1, (0, 255, 0))
cv2.circle(numpy.asarray(frame), (320,180), offset, (0, 0, 255))
elif self.face_detected==1:
self.face_detected=0
if (self.face_detected==0):
frame,center_point=self.track_object(cv.fromarray(frame))
if framecount%1==0 and 'center_point' in locals():
self.motor.move_to_pos(center_point,[320,180],offset,[2,2])
if show:
cv2.imshow('FlatBuddy', numpy.asarray(frame))
framecount=framecount+1
key=cv2.waitKey(10)
def callback(data):
img=np.zeros((250,500,3), np.uint8)
#system output
font=cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0,thickness=2)
cv.PutText(cv.fromarray(img), "System output", (20,50), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), "2D + 3D", (20,100), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), ":", (350,75), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), str(data.id_2d_plus_3d), (450,75), font, (255, 255, 255))
#2d
cv.PutText(cv.fromarray(img), "2D", (20,150), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), ":", (350,150), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), str(data.id_2d), (450,150), font, (255, 255, 255))
#3d
cv.PutText(cv.fromarray(img), "3D", (20,200), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), ":", (350,200), font, (255, 255, 255))
cv.PutText(cv.fromarray(img), str(data.id_3d), (450,200), font, (255, 255, 255))
cv2.imshow("SYSTEM OUTPUT", img)
cv2.waitKey(3)
def imtransform(I, H0, fillval=np.nan):
# transform image using center as origin
if len(I.shape) == 3:
Iout = np.copy(I)
Iout[:, :, 0] = imtransform(I[:, :, 0], H0, fillval=fillval)
Iout[:, :, 1] = imtransform(I[:, :, 1], H0, fillval=fillval)
Iout[:, :, 2] = imtransform(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
if not I.flags.c_contiguous:
I = np.copy(I)
Icv = cv.fromarray(I)
I1cv = cv.CreateMat(I.shape[0], I.shape[1], Icv.type)
H = H[:2]
H = cv.fromarray(H)
# cv.WarpPerspective(Icv,I1cv,cv.fromarray(np.copy(H)),fillval=-1);
cv.WarpAffine(Icv, I1cv, H) # ,fillval=fillval);
I1 = np.asarray(I1cv)
# I1[np.nonzero(I1<0)]=fillval
return I1
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 simplest_color_balance(image, s1=0, s2=0, mask=None):
#tick = time.time()
UMAX = 255
copy = image.copy()
layers = cv2.split(copy)
for i, layer in enumerate(layers):
hist = cv2.calcHist([layer], [0], mask, [UMAX + 1], [0, UMAX])
cumsum = hist.cumsum()
size = cumsum[-1]
lb = size * s1 / 100.0
ub = size * (1 - s2 / 100.0)
# reinit borders
left = bisect.bisect_left(cumsum, lb)
right = bisect.bisect_right(cumsum, ub)
if (right - left) <= 0:
left = UMAX / 2 - 1
right = UMAX / 2
#print left, right
# replacing values
if left != 0:
left_mask = cv2.inRange(layer, np.array(0), np.array(left))
cv.Set(cv.fromarray(layer), left, cv.fromarray(left_mask))
if right != UMAX:
right_mask = cv2.inRange(layer, np.array(right), np.array(UMAX))
cv.Set(cv.fromarray(layer), right, cv.fromarray(right_mask))
layers[i] = layer
layers = map(lambda x: cv2.normalize(x, x, 0, UMAX, cv2.NORM_MINMAX),
layers)
copy = cv2.merge(layers)
#cv2.imshow('filter', copy)
#tock = time.time()
#print tock - tick
return copy
def __init__(self):
# Initial process state
self.x = np.array([0, 0]).T
# Process noise (TODO estimate it. ALS technique)
self.Q = np.eye(2) * 1e-4
# Observation noise
self.R = np.eye(3) * 1e-2
# Estimate covariance
self.P = np.zeros_like(self.Q)
# Time at which the last observation has been taken
self.last_obs = time.time()
# Camera calibration matrix
self.K = np.array([[353.526260, 0.000000, 190.146881],
[0.000000, 356.349156, 138.256491],
[0.000000, 0.000000, 1.000000]])
# Inverse calibration matrix
self.Kinv = np.linalg.inv(self.K)
# Matrices for undistort
self.cv_camera_matrix = cv.fromarray(self.K)
self.distortion_coefficients = cv.fromarray(
np.matrix([0.053961, -0.153046, 0.001022, 0.017833, 0.0000]))
# Camera frame with respect to fixed frame
self.Pcf = np.array([0.0, 0.32, 0.48]).T
# Length of the string
self.l = 0.30
def localize(self, tags):
global arr_ball_pos
tag_positions = Tag_Positions()
for t in tags.tags:
current_tag_position = Tag_Position()
current_tag_position.id = t.id
#This is all Open CV matrix stuff that is more complicated
#than it should be.
a = cv.fromarray(numpy.array([[[float(t.x), float(t.y)]]]))
b = cv.fromarray(numpy.empty((1, 1, 2)))
cv.PerspectiveTransform(a, b, self.transform)
current_tag_position.x = numpy.asarray(b)[0, 0, 0]
current_tag_position.y = numpy.asarray(b)[0, 0, 1]
current_tag_position.theta = t.zRot - self.zRot_offset
#Adjust tag angle based on its position
current_tag_position.theta += correct(current_tag_position.x,
current_tag_position.y,
arr_correct_theta)
if (current_tag_position.theta < -pi):
current_tag_position.theta += 2 * pi
if (current_tag_position.theta > pi):
current_tag_position.theta -= 2 * pi
tag_positions.tag_positions.append(current_tag_position)
for tcache in arr_ball_pos:
tag_positions.tag_positions.append(tcache)
self.pub.publish(tag_positions)
def UpdateWingMask(self, npfRoiMean):
#global globalLock
self.npMaskWings = N.zeros([self.heightRoi, self.widthRoi], dtype=N.uint8)
# Coordinates of the body ellipse.
centerBody = (self.widthRoi/2,
self.heightRoi/2)
sizeBody = (self.lengthBody,
self.widthBody)
angleBody = 0.0
# Left wing ellipse.
sizeLeft = (self.lengthBody*10/10,
self.lengthBody*7/10)
centerLeft = (centerBody[0] - sizeLeft[0]/2 + self.lengthBody*1/10,
centerBody[1] - sizeLeft[1]/2 - 1)
angleLeft = 0.0
# Right wing ellipse.
sizeRight = (self.lengthBody*10/10,
self.lengthBody*7/10)
centerRight = (centerBody[0] - sizeRight[0]/2 + self.lengthBody*1/10,
centerBody[1] + sizeRight[1]/2)
angleRight = 0.0
# Draw ellipses on the mask.
#cv2.ellipse(self.npMaskWings,
# (centerBody, sizeBody, angleBody),
# 255, cv.CV_FILLED)
cv2.ellipse(self.npMaskWings,
(centerLeft, sizeLeft, angleLeft*180.0/N.pi),
255, cv.CV_FILLED)
cv2.ellipse(self.npMaskWings,
(centerRight, sizeRight, angleRight*180.0/N.pi),
255, cv.CV_FILLED)
if (npfRoiMean is not None):
# Mean Fly Body Mask.
#with globalLock:
# self.thresholdForeground = rospy.get_param('tracking/thresholdForeground', 25.0)
(threshOut, npMaskBody) = cv2.threshold(npfRoiMean.astype(N.uint8), self.thresholdForeground, 255, cv2.THRESH_BINARY_INV)
self.npMaskWings = cv2.bitwise_and(self.npMaskWings, npMaskBody)
# Publish non-essential stuff.
if globalNonessential:
npMaskWings1 = copy.copy(self.npMaskWings)
npMaskWings1.resize(npMaskWings1.size)
imgMaskWings = self.cvbridge.cv_to_imgmsg(cv.fromarray(self.npMaskWings), 'passthrough')
imgMaskWings.data = list(npMaskWings1)
self.pubImageMask.publish(imgMaskWings)
npMaskBody1 = copy.copy(npMaskBody)
npMaskBody1.resize(npMaskBody1.size)
imgMaskBody = self.cvbridge.cv_to_imgmsg(cv.fromarray(npMaskBody), 'passthrough')
imgMaskBody.data = list(npMaskBody1)
self.pubImageMaskBody.publish(imgMaskBody)
def localize(self, tags):
global arr_ball_pos
tag_positions = Tag_Positions()
for t in tags.tags:
current_tag_position = Tag_Position()
current_tag_position.id = t.id
#This is all Open CV matrix stuff that is more complicated
#than it should be.
a = cv.fromarray(numpy.array([[[float(t.x), float(t.y)]]]))
b = cv.fromarray(numpy.empty((1,1,2)))
cv.PerspectiveTransform(a, b, self.transform)
current_tag_position.x = numpy.asarray(b)[0,0,0]
current_tag_position.y = numpy.asarray(b)[0,0,1]
current_tag_position.theta = t.zRot - self.zRot_offset
#Adjust tag angle based on its position
current_tag_position.theta += correct(current_tag_position.x, current_tag_position.y, arr_correct_theta)
if (current_tag_position.theta < -pi):
current_tag_position.theta += 2*pi
if (current_tag_position.theta > pi):
current_tag_position.theta -= 2*pi
tag_positions.tag_positions.append(current_tag_position)
for tcache in arr_ball_pos:
tag_positions.tag_positions.append(tcache)
self.pub.publish(tag_positions)
def simplest_color_balance(image, s1=0, s2=0, mask=None):
#tick = time.time()
UMAX = 255
copy = image.copy()
layers = cv2.split(copy)
for i, layer in enumerate(layers):
hist = cv2.calcHist([layer], [0], mask, [UMAX + 1], [0, UMAX])
cumsum = hist.cumsum()
size = cumsum[-1]
lb = size * s1 / 100.0
ub = size * (1 - s2 / 100.0)
# reinit borders
left = bisect.bisect_left(cumsum, lb)
right = bisect.bisect_right(cumsum, ub)
if (right - left) <= 0:
left = UMAX / 2 - 1
right = UMAX / 2
#print left, right
# replacing values
if left != 0:
left_mask = cv2.inRange(layer, np.array(0), np.array(left))
cv.Set(cv.fromarray(layer), left, cv.fromarray(left_mask))
if right != UMAX:
right_mask = cv2.inRange(layer, np.array(right), np.array(UMAX))
cv.Set(cv.fromarray(layer), right, cv.fromarray(right_mask))
layers[i] = layer
layers = map(lambda x: cv2.normalize(x, x, 0, UMAX, cv2.NORM_MINMAX), layers)
copy = cv2.merge(layers)
#cv2.imshow('filter', copy)
#tock = time.time()
#print tock - tick
return copy
def __init__(self,image_raw='image_raw',pattern=None,KK=None,kc=None,timeout=4.0):
self.image_raw=image_raw
self.bridge = CvBridge()
self.pattern=pattern
px=self.pattern['corners_x']*self.pattern['spacing_x']
py=self.pattern['corners_y']*self.pattern['spacing_y']
self.pattern['type'] = """<KinBody name="calibration">
<Body name="calibration">
<Geom type="box">
<extents>%f %f 0.001</extents>
<translation>%f %f 0</translation>
<diffusecolor>0 0.5 0</diffusecolor>
</Geom>
<Geom type="box">
<extents>%f %f 0.002</extents>
<translation>%f %f 0</translation>
<diffusecolor>0 1 0</diffusecolor>
</Geom>
</Body>
</KinBody>
"""%(px*0.5+2.0*self.pattern['spacing_x'],py*0.5+2.0*self.pattern['spacing_y'],px*0.5,py*0.5,px*0.5,py*0.5,px*0.5,py*0.5)
self.obstaclexml = """<KinBody name="obstacle">
<Body name="obstacle">
<Geom type="box">
<extents>%f %f 0.001</extents>
<translation>0 0 0.001</translation>
<diffusecolor>1 0 0</diffusecolor>
</Geom>
</Body>
</KinBody>"""%(px+0.1,py+0.1)
self.timeout=timeout
self.cvKK = None if KK is None else cv.fromarray(KK)
self.cvkc = None if kc is None else cv.fromarray(kc)
def dewarp(imagedir):
# Loading from json file
C = CameraParams()
C.load(imagedir+"/params.json")
K = cv.fromarray(C.K)
D = cv.fromarray(C.D)
print "loaded camera parameters"
mapx = None
mapy = None
for f in os.listdir(imagedir):
if (f.find('pgm')<0):
continue
image = imagedir+'/'+f
print image
original = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE)
dewarped = cv.CloneImage(original);
# setup undistort map for first time
if (mapx == None or mapy == None):
im_dims = (original.width, original.height)
mapx = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
mapy = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
cv.InitUndistortMap(K,D,mapx,mapy)
cv.Remap( original, dewarped, mapx, mapy )
tmp1=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(original,tmp1)
tmp2=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(dewarped,tmp2)
cv.ShowImage("Original", tmp1 )
cv.ShowImage("Dewarped", tmp2)
cv.WaitKey(-1)
def detcb(imagemsg):
if imagemsg.header.stamp > timestart:
cvimage = self.bridge.imgmsg_to_cv(imagemsg, 'mono8')
corners = self.detect(cvimage)
if corners is not None:
with donecond:
# get the pose with respect to the attached sensor tf frame
if self.cvKK is None or self.cvkc is None:
KK, kc, Ts, error = self.calibrateIntrinsicCamera(
[self.getObjectPoints()], [corners],
(cvimage.width, cvimage.height),
usenonlinoptim=False)
T = Ts[0]
else:
cvrvec = cv.CreateMat(1, 3, cv.CV_64F)
cvtvec = cv.CreateMat(1, 3, cv.CV_64F)
cv.FindExtrinsicCameraParams2(
cv.fromarray(self.getObjectPoints()),
cv.fromarray(corners), self.cvKK, self.cvkc,
cvrvec, cvtvec)
T = matrixFromAxisAngle(array(cvrvec)[0])
T[0:3, 3] = array(cvtvec)[0]
data[0] = {
'T': T,
'corners_raw': corners,
'image_raw': array(cvimage)
}
if 'type' in self.pattern:
data[0]['type'] = self.pattern['type']
donecond.notifyAll()
def capture_rgb(): rgb_frame = numpy.fromstring(image_generator.get_raw_image_map_bgr(), dtype=numpy.uint8).reshape(480, 640, 3) image = cv.fromarray(rgb_frame) cv.CvtColor(cv.fromarray(rgb_frame), image, cv.CV_BGR2RGB) pyimage = pygame.image.frombuffer(image.tostring(), cv.GetSize(image), 'RGB') return pyimage
def step(self, pause=False):
try:
retval, img_arr = self.cam.read()
#cv2.imwrite(settings.base_path+'pics/pic1.jpg', img_arr)
assert img_arr is not None, "Camera in use by other process"
self.add_observation(img_arr, draw=self.show_overlays)
if settings.use_simplecv_display:
if self.display.isDone():
raise SystemExit, "exiting"
img = Image(cv.fromarray(img_arr))
if self.show_overlays or self.trainable:
self.annotate_img(img)
img.save(self.display)
if self.display.mouseLeft:
self.show_overlays = not self.show_overlays
if self.display.mouseRight:
self.display.done = True
else:
cv.ShowImage("Index", cv.fromarray(img_arr))
if pause:
print("Press any key to continue")
cv.WaitKey()
cv.DestroyAllWindows()
return True
except KeyboardInterrupt:
return False
def EarthMovers_dist(s1, s2):
x = np.array(s1)
y = np.array(s2)
#print("x = ", x ," y = " , y)
a = np.zeros((len(x), 2))
b = np.zeros((len(y), 2))
for i in range(0, len(a)):
a[i][0] = x[i]
a[i][1] = i + 1.0
for i in range(0, len(b)):
b[i][0] = y[i]
b[i][1] = i + 1.0
#print("a = ", a ," b = " , b)
# Convert from numpy array to CV_32FC1 Mat
a64 = cv.fromarray(a)
a32 = cv.CreateMat(a64.rows, a64.cols, cv.CV_32FC1)
cv.Convert(a64, a32)
b64 = cv.fromarray(b)
b32 = cv.CreateMat(b64.rows, b64.cols, cv.CV_32FC1)
cv.Convert(b64, b32)
# Calculate Earth Mover's
dis = cv.CalcEMD2(
a32, b32, cv.CV_DIST_L1
) #CV_DIST_L2 -- Euclidean Distance, CV_DIST_L1 --- Manhattan Distance
return dis
def remap(img, mapy, mapx, interp=2):
"""
transform image using coordinate x,y
Interpolation method:
0 = CV_INTER_NN nearest-neigbor interpolation
1 = CV_INTER_LINEAR bilinear interpolation (used by default)
2 = CV_INTER_CUBIC bicubic interpolation
3 = CV_INTER_AREA resampling using pixel area relation. It is the preferred method for image decimation that gives moire-free results. In terms of zooming it is similar to the CV_INTER_NN method
return resulting array
"""
des = N.empty_like(img)
# cv.fromarray: array can be 2D or 3D only
if cv2.__version__.startswith('2'):
cimg = cv.fromarray(img)
cdes = cv.fromarray(des)
cmapx = cv.fromarray(mapx.astype(N.float32))
cmapy = cv.fromarray(mapy.astype(N.float32))
cv.Remap(cimg, cdes, cmapx, cmapy, flags=interp+cv.CV_WARP_FILL_OUTLIERS)
else:
cimg = img
cdes = des
cmapx = mapx.astype(N.float32)
cmapy = mapy.astype(N.float32)
cdes = cv2.remap(cimg, cmapx, cmapy, interp)
return N.asarray(cdes)
def LoadImage(filename, rotate180=False, RGB=False):
'''wrapper around cv.LoadImage that also handles PGM.
It always returns a colour image of the same size'''
if filename.endswith('.pgm'):
from ..image import scanner
try:
pgm = PGM(filename)
except Exception as e:
print('Failed to load %s: %s' % (filename, e))
return None
im_full = numpy.zeros((960, 1280, 3), dtype='uint8')
if RGB:
scanner.debayer_RGB(pgm.array, im_full)
else:
scanner.debayer(pgm.array, im_full)
if rotate180:
scanner.rotate180(im_full)
return cv.GetImage(cv.fromarray(im_full))
img = cv.LoadImage(filename)
if rotate180:
from ..image import scanner
img = numpy.ascontiguousarray(cv.GetMat(img))
scanner.rotate180(img)
img = cv.GetImage(cv.fromarray(img))
return img
def backproject_sample_rect(warp_matrix, sample_dims):
warp_matrix_inv = cv2.invert(warp_matrix)[1]
#warp_matrix_inv = np.array(warp_matrix_inv)/cv.Get2D(warp_matrix_inv, 2, 2)[0]
#print warp_matrix_inv
warp_matrix_inv = cv.fromarray(warp_matrix_inv)
rectified_shape = np.array((1., 1.))
width_center = rectified_shape[1] / 2
sample_dims = [x / 2 for x in sample_dims]
#roi_bottom = (rectified_shape[0] - neighborhood_length) * np.random.random() + \
# neighborhood_length
roi_bottom = 0.5
rectified_sample_roi = [
width_center - sample_dims[0], roi_bottom + sample_dims[1],
width_center - sample_dims[0], roi_bottom - sample_dims[1],
width_center + sample_dims[0], roi_bottom - sample_dims[1],
width_center + sample_dims[0], roi_bottom + sample_dims[1]
]
rectified_sample_roi = np.array(rectified_sample_roi, dtype=np.float32)
rectified_sample_roi = rectified_sample_roi.reshape((1, 4, 2))
backprojected_sample_boundary = cv.CreateMat(1, 4, cv.CV_32FC2)
rectified_sample_roi = cv.fromarray(rectified_sample_roi)
cv.PerspectiveTransform(rectified_sample_roi,
backprojected_sample_boundary, warp_matrix_inv)
return cvutils.array2point_list(np.array(backprojected_sample_boundary))
def __init__(self):
# Initial process state
self.x = np.array([0, 0]).T
# Process noise (TODO estimate it. ALS technique)
self.Q = np.eye(2) * 1e-4
# Observation noise
self.R = np.eye(3) * 1e-2
# Estimate covariance
self.P = np.zeros_like(self.Q)
# Time at which the last observation has been taken
self.last_obs = time.time()
# Camera calibration matrix
self.K = np.array(
[[353.526260, 0.000000, 190.146881], [0.000000, 356.349156, 138.256491], [0.000000, 0.000000, 1.000000]]
)
# Inverse calibration matrix
self.Kinv = np.linalg.inv(self.K)
# Matrices for undistort
self.cv_camera_matrix = cv.fromarray(self.K)
self.distortion_coefficients = cv.fromarray(np.matrix([0.053961, -0.153046, 0.001022, 0.017833, 0.0000]))
# Camera frame with respect to fixed frame
self.Pcf = np.array([0.0, 0.32, 0.48]).T
# Length of the string
self.l = 0.30
def unstuffBag(self):
bag = rosbag.Bag(self.bag_name)
saved_camera_info = False
img_n = 0
pano_dir = tempfile.mkdtemp("pano")
image_names = []
for topic, msg, t in bag.read_messages(topics=['image', 'camera_info']):
if ('image' in topic):
try:
cv_image = self.bridge.imgmsg_to_cv(msg, "rgb8")
image_name = "%s/image_%05d.png" % (pano_dir , img_n)
image_names.append("image_%05d.png" % img_n)
cv.SaveImage(image_name,cv_image)
rospy.loginfo("saving image to %s",image_name)
img_n += 1
except CvBridgeError, e:
print e
if ('camera_info' in topic and not saved_camera_info):
mK = numpy.array(msg.K)
mK = numpy.reshape(mK, (3,3), 'C' )
K = cv.fromarray(mK)
mD = numpy.array(msg.D)
mD = numpy.reshape(mD, (5,1), 'C' )
D = cv.fromarray(mD)
cv.Save(pano_dir +"/K.yml",K)
cv.Save(pano_dir +"/D.yml",D)
saved_camera_info = True
def processing(input, args):
'''DO ALL PROCESSING IN HERE...'''
output = collections.namedtuple('Processing', ['phase_out', 'canny'])
if input.channels != 1:
# Convert to greyscale
grey = cv.CreateMat(input.height, input.width, cv.CV_8UC1)
cv.CvtColor(input, grey, cv.CV_RGB2GRAY)
else:
grey = input
# cv.Smooth(grey, smooth, 19)
# smooth = cv.CreateMat(input.height, input.width, cv.CV_8UC1)
# Simple Canny edge detection
canny_edges = cv.CreateMat(input.height, input.width, cv.CV_8UC1)
if args.canny:
cv.Canny(grey, canny_edges, 70, 100)
# Phase congruency calculation
# First onvert image to numpy array
im = np.asarray(grey)
phase_data = phase.phasecong(im, noiseMethod = args.noise)
if args.corners:
phase_out = cv.fromarray(phase_data.m)
else:
phase_out = cv.fromarray(phase_data.M)
return output(phase_out, canny_edges)
def capture_rgb_frame(self): frame = numpy.fromstring(self.image_generator.get_raw_image_map_bgr(), dtype=numpy.uint8).reshape(480, 640, 3) image = cv.fromarray(frame) cv.CvtColor(cv.fromarray(frame), image, cv.CV_BGR2RGB) pygame_image = pygame.image.frombuffer(image.tostring(), cv.GetSize(image), 'RGB') return pygame_image
def get_target_pose(cam):
# Populate object_points
object_points = cv.fromarray(
numpy.array([[p.x, p.y, p.z] for p in cam.features.object_points]))
image_points = cv.fromarray(
numpy.array([[p.x, p.y] for p in cam.features.image_points]))
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[[cam.cam_info.P[0], cam.cam_info.P[1], cam.cam_info.P[2]],
[cam.cam_info.P[4], cam.cam_info.P[5], cam.cam_info.P[6]],
[cam.cam_info.P[8], cam.cam_info.P[9], cam.cam_info.P[10]]])
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(object_points, image_points,
cv.fromarray(camera_matrix), dist_coeffs,
rot, trans)
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
frame = PyKDL.Frame()
for i in range(3):
frame.p[i] = trans[i, 0]
for i in range(3):
for j in range(3):
frame.M[i, j] = rot3x3[i, j]
return frame
def numpy_to_iplimage_color(numpy_format):
cvMat = cv.fromarray(numpy_format)
cv.SaveImage('cvMat.png', cvMat) #Saves the image
iplimage = cv.LoadImage('cvMat.png', cv.CV_LOAD_IMAGE_COLOR)
f**k = cv.fromarray(numpy_format)
os.system('rm cvMat.png')
return iplimage
def run ( self ):
#set up opencv windows
cv.NamedWindow("Camera", 1)
cv.NamedWindow("Binary", 1)
cv.NamedWindow("Settings", 1)
#set up sliders
cv.CreateTrackbar("Hue", "Settings", hue, 180, on_hueTrackbar)
cv.CreateTrackbar("Sat", "Settings", sat, 255, on_satTrackbar)
cv.CreateTrackbar("Val", "Settings", val, 255, on_valTrackbar)
#run a blocking while loop to capture depth and rgb to opencv window
while 1:
#pull in camera data
(depth,_),(rgb,_)=freenect.sync_get_depth(),freenect.sync_get_video()
depth=depth.astype(np.uint8)
h1, w1 = depth.shape[:2]
h2, w2 = rgb.shape[:2]
maxHeight= max(h1,h2)
vis = np.zeros((maxHeight, w1+w2), np.uint8)
vis2 = np.zeros((h2,w2), np.uint8)
cv.CvtColor(cv.fromarray(rgb), cv.fromarray(vis2), cv.CV_BGR2GRAY)
#display in a single window
vis[:maxHeight, :w1] = depth
vis[:maxHeight, w1:w1+w2] = vis2
cv.ShowImage("Camera",cv.fromarray(vis))
cv.WaitKey(100)
def gotimage(self, imgmsg):
if imgmsg.encoding == "bayer_bggr8":
imgmsg.encoding = "8UC1"
img = CvBridge().imgmsg_to_cv(imgmsg)
# cv.ShowImage("DataMatrix", img)
# cv.WaitKey(6)
self.track(img)
# monocular case
print self.cams
if 'l' in self.cams:
for (code, corners) in self.tracking.items():
model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(model,
cv.fromarray(numpy.array(corners, numpy.float32)),
self.cams['l'].intrinsicMatrix(),
self.cams['l'].distortionCoeffs(),
rot,
trans)
ts = geometry_msgs.msg.TransformStamped()
ts.header.frame_id = imgmsg.header.frame_id
ts.header.stamp = imgmsg.header.stamp
ts.child_frame_id = code
posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
ts.transform.translation = posemsg.position
ts.transform.rotation = posemsg.orientation
tfm = tf.msg.tfMessage([ts])
self.pub_tf.publish(tfm)
def draw(self, vis):
self.adjustCamera()
for i in xrange(len(self.colors)):
(center, norm, up, right) = self.faceInfo(i)
if norm**(center - self.camera.pos) < 0:
corners = (center + up * 0.5 + right * 0.5,
center + up * 0.5 - right * 0.5,
center - up * 0.5 - right * 0.5,
center - up * 0.5 + right * 0.5)
corners = [corner.flatten(self.camera) for corner in corners]
corners = [(int(corner[0]), int(corner[1]))
for corner in corners]
cv.FillConvexPoly(cv.fromarray(vis),
corners,
colorTuple(self.colors[i]),
lineType=4,
shift=0)
for i in xrange(len(self.colors)):
(center, norm, up, right) = self.faceInfo(i)
if norm**(center - self.camera.pos) < 0:
corners = (center + up * 0.5 + right * 0.5,
center + up * 0.5 - right * 0.5,
center - up * 0.5 - right * 0.5,
center - up * 0.5 + right * 0.5)
corners = [corner.flatten(self.camera) for corner in corners]
corners = [(int(corner[0]), int(corner[1]))
for corner in corners]
for j in xrange(len(corners)):
k = (j + 1) % (len(corners))
cv.Line(cv.fromarray(vis), corners[j], corners[k],
(0, 0, 0))
def draw(self, vis):
self.adjustCamera()
for i in xrange(len(self.colors)):
(center, norm, up, right) = self.faceInfo(i)
if norm ** (center - self.camera.pos) < 0:
corners = (center + up * 0.5 + right * 0.5,
center + up * 0.5 - right * 0.5,
center - up * 0.5 - right * 0.5,
center - up * 0.5 + right * 0.5)
corners = [corner.flatten(self.camera) for corner in corners]
corners = [(int(corner[0]), int(corner[1])) for corner in corners]
cv.FillConvexPoly(cv.fromarray(vis),
corners, colorTuple(self.colors[i]), lineType=4, shift=0)
for i in xrange(len(self.colors)):
(center, norm, up, right) = self.faceInfo(i)
if norm ** (center - self.camera.pos) < 0:
corners = (center + up * 0.5 + right * 0.5,
center + up * 0.5 - right * 0.5,
center - up * 0.5 - right * 0.5,
center - up * 0.5 + right * 0.5)
corners = [corner.flatten(self.camera) for corner in corners]
corners = [(int(corner[0]), int(corner[1]))
for corner in corners]
for j in xrange(len(corners)):
k = (j + 1) % (len(corners))
cv.Line(cv.fromarray(vis), corners[j], corners[k], (0,0,0))
def get_coefficients(self, **kwargs):
# Create a [k,x,y]-list containing our radial and circular masks.
masks = [mask.radial(**kwargs), mask.circular(**kwargs)]
# Prepare a data structure to store our coefficients.
coefficients = [
numpy.empty(m.shape[:1] + self.image.shape, dtype=numpy.complex)
for m in masks
]
for mask_type, mask_coefficients in zip(masks, coefficients):
for m, coefficient in zip(mask_type, mask_coefficients):
# Create storage arrays for our real and imaginary components.
real_coef = cv.CreateMat(self.image.shape[0],
self.image.shape[1], cv.CV_64FC1)
imag_coef = cv.CreateMat(self.image.shape[0],
self.image.shape[1], cv.CV_64FC1)
# Filter our image using our masks.
cv.Filter2D(cv.fromarray(self.image.astype(numpy.float64)),
real_coef, cv.fromarray(m.real.copy()))
cv.Filter2D(cv.fromarray(self.image.astype(numpy.float64)),
imag_coef, cv.fromarray(m.real.copy()))
# Store our coefficients.
coefficient[:] = numpy.asarray(
real_coef) + 1j * numpy.asarray(imag_coef)
return coefficients
def plot_intercontour_hist(image, outer_contour_id, contours, graph, normalized=True):
outer_contour = contours[outer_contour_id]
(x, y, width, height) = cv2.boundingRect(outer_contour)
subimage = get_subimage(image, (x, y), (x + width, y + height))
monochrome = cv2.cvtColor(subimage, cv2.COLOR_BGR2GRAY)
inverted_mask = cv2.compare(monochrome, monochrome, cv2.CMP_EQ)
inner_contours = [contours[int(contour_id)] for contour_id in graph.successors(outer_contour_id)]
for i in range(width):
for j in range(height):
point = (x + i, y + j)
outer_contour_test = cv2.pointPolygonTest(outer_contour, point, 0)
inner_contours_test = -1
for inner_contour in inner_contours:
inner_contour_test = cv2.pointPolygonTest(inner_contour, point, 0)
if inner_contour_test > 0:
inner_contours_test = 1
break
if outer_contour_test >= 0 and inner_contours_test < 0:
inverted_mask[j][i] = 0
mask = cv2.bitwise_not(inverted_mask)
cv.Set(cv.fromarray(subimage), WHITE, cv.fromarray(inverted_mask))
inner_contour_id = len(str(inner_contours))
print 'inner contour id: ', inner_contour_id
image_name = '%d-%s'%(outer_contour_id, inner_contour_id)
#cv2.imshow(image_name, subimage)
#subhists = plot_hist(subimage, mask, image_name)
(subhists, winnames) = plot_hist_hls(subimage, mask, image_name, normalized)
return subhists, subimage, mask, x, y, winnames
def genView(panopath, outpath, orientation, viewdir, detail):
# panopath: location of raster, depth and plane_pano images
# outpath: location to put generated view, depth, and plane images
# orientation: [yaw, pitch, roll] of panorama (in degrees)
# viewdir: clock-based view; in set [2,3,4,8,9,10] for database
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
start = time.time()
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# view details
Rpano = geom.RfromYPR(orientation[0],orientation[1],orientation[2])
Yview = np.mod( orientation[0] + 30*viewdir, 360 )
Rview = geom.RfromYPR(Yview, 0, 0)
Kview = geom.cameramat(width, height, fov*pi/180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM( os.path.join(panopath,'raster.jpg'), cv.CV_LOAD_IMAGE_UNCHANGED )
cvDP = cv.fromarray( np.asarray( Image.open( os.path.join(panopath,'depth.png') ) ) )
pp = np.asarray( Image.open( os.path.join(panopath,'plane_pano.png') ) ).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp==gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(np.array(np.meshgrid(range(width),range(height))).reshape(2,-1),np.ones([1,width*height]),0)
midpoint = time.time()
print 'Loading pano images took ' + str(midpoint-start) + ' seconds.'
# Create output openCV matrices
cvI = cv.CreateMat(height,width,cv.CV_8UC3)
cvD = cv.CreateMat(height,width,cv.CV_32SC1)
cvP = cv.CreateMat(height,width,cv.CV_8UC1)
# compute mappings
ray = np.dot( np.transpose(Rpano), np.dot( Rview, np.dot( Kinv, pix ) ) )
yaw, pitch = np.arctan2( ray[0,:] , ray[2,:] ) , np.arctan2( -ray[1,:] , np.sqrt((np.array([ray[0,:],ray[2,:]])**2).sum(0)) )
ix, iy = cv.fromarray(np.array(8192/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(4096/pi*(pi/2-pitch),np.float32).reshape(height,width))
dx, dy = cv.fromarray(np.array(5000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(2500/pi*(pi/2-pitch),np.float32).reshape(height,width))
px, py = cv.fromarray(np.array(1000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array( 500/pi*(pi/2-pitch),np.float32).reshape(height,width))
# call remap function
cv.Remap(cvIP,cvI,ix,iy,cv.CV_INTER_CUBIC)
cv.Remap(cvDP,cvD,dx,dy,cv.CV_INTER_NN)
cv.Remap(cvPP,cvP,px,py,cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:,:,[2,1,0]]).save(os.path.join(outpath,'view.jpg'),'jpeg')
Image.fromarray(np.array(cvD)).save(os.path.join(outpath,'depth.png'),'png')
Image.fromarray(np.array(cvP)).save(os.path.join(outpath,'plane.png'),'png')
print 'Generating views from pano took ' + str(time.time()-midpoint) + ' seconds.'
def colour_backproject(target, image, bins, model):
"""
Implementation of "object location via histogram backprojection" algorithm
in Swain and Ballard's 1990 paper. Uses OpenCV library Filter2D to carry out
covolution.
"""
print '\nFirst, we build the target\'s colour histogram:'
press_enter_and_wait()
target_h = col_hist(target, bins, model)
print target_h
print '\nNext, we build the image\'s colour histogram:'
press_enter_and_wait()
image_h = col_hist(image, bins, model)
print image_h
print '\nNow, we compute "ratio", the ratio of target/image histograms:'
press_enter_and_wait()
ratio = (target_h * 1.0) / image_h
print ratio
print '\nNext, we replace each pixel in the image with the value of the',
print 'bin in "ratio" that the pixel\'s colour indexes (max value: 1).'
press_enter_and_wait()
b = np.empty(((image.shape)[0:2]), dtype = float)
i = 0
for row in image:
j = 0
for pixel in row:
index = col2bin(pixel, bins, model)
b[i][j] = min(ratio[index[0]][index[1]][index[2]], 1)
j += 1
i += 1
print b
print '\nOk, now to convolve a mask with b'
press_enter_and_wait()
b_conv = b
radius = 40
w = np.zeros((81,81), dtype = float)
i = 0
for row in w:
j = 0
for element in row:
if (sqrt(pow((10-i),2)+pow((10-j),2)) < radius):
w[i][j] = 1
j += 1
i += 1
cv.Filter2D(cv.fromarray(b), cv.fromarray(b_conv), cv.fromarray(w), (-1, -1))
b_conv = np.array(b_conv)
print b_conv
print '\nFinally, we find the location of the peak of the convolved image:'
press_enter_and_wait()
print b_conv
return np.where(b_conv == b_conv.max()), b_conv
def image_callback(data):
global running
if (running):
image = bridge.imgmsg_to_cv(data, "bgr8")
#normalize image
cv.Split(image, rgb_r, rgb_g, rgb_b, None)
red_mean = cv2.mean(np.asarray(rgb_r[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_r,
dst=scaled_r,
scale=128 / red_mean[0])
green_mean = cv2.mean(np.asarray(rgb_g[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_g,
dst=scaled_g,
scale=128 / green_mean[0])
blue_mean = cv2.mean(np.asarray(rgb_b[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_b,
dst=scaled_b,
scale=128 / blue_mean[0])
cv.Merge(scaled_r, scaled_g, scaled_b, None, cv_image)
cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV) # --convert from BGR to HSV
cv.CvtColor(cv_image, lab, cv.CV_BGR2Lab)
cv.Split(hsv, hsv_h, hsv_s, hsv_v, None)
cv.Split(cv_image, rgb_r, rgb_g, rgb_b, None)
cv.Split(lab, lab_l, lab_a, lab_b, None)
cv.Split(luv, luv_l, luv_u, luv_v, None)
cv.Split(hls, hls_h, hls_l, hls_s, None)
cv.Split(xyz, xyz_x, xyz_y, xyz_x, None)
cv.Split(ycrcb, ycrcb_y, ycrcb_cr, ycrcb_cb, None)
cv.Not(lab_a, a_not)
cv.Sub(hsv_s, a_not, sa)
cv.Sub(luv_u, hls_h, test)
cv.Sub(hls_s, hls_h, sminh)
threshold_red(sa)
cv.ShowImage("red", red_dilated_image)
red_contours, _ = cv2.findContours(image=np.asarray(
red_dilated_image[:, :]),
mode=cv.CV_RETR_EXTERNAL,
method=cv.CV_CHAIN_APPROX_SIMPLE)
print_lidar_projections(cv_image)
circles = extract_circles(red_contours, [1, 0, 0])
for x, y, radius in circles:
cv.Circle(cv_image, (x, y), radius, [0, 0, 1], 3)
cv.SetMouseCallback("camera feed", mouse_callback, hsv_image)
cv.ShowImage("camera feed", cv_image)
cv.WaitKey(3)
def handle_monocular(self, msg):
def pt2line(x0, y0, x1, y1, x2, y2):
""" point is (x0, y0), line is (x1, y1, x2, y2) """
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) *
(y2 - y1)) / math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
(image, camera) = msg
rgb = self.mkgray(image)
C = self.image_corners(rgb)
if C:
cc = self.board.n_cols
cr = self.board.n_rows
errors = []
for r in range(cr):
(x1, y1) = C[(cc * r) + 0]
(x2, y2) = C[(cc * r) + cc - 1]
for i in range(1, cc - 1):
(x0, y0) = C[(cc * r) + i]
errors.append(pt2line(x0, y0, x1, y1, x2, y2))
linearity_rms = math.sqrt(
sum([e**2 for e in errors]) / len(errors))
# Add in reprojection check
A = cv.CreateMat(3, 3, 0)
image_points = cv.fromarray(numpy.array(C))
object_points = cv.fromarray(numpy.zeros([cc * cr, 3]))
for i in range(cr):
for j in range(cc):
object_points[i * cc + j, 0] = j * self.board.dim
object_points[i * cc + j, 1] = i * self.board.dim
object_points[i * cc + j, 2] = 0.0
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[[camera.P[0], camera.P[1], camera.P[2]],
[camera.P[4], camera.P[5], camera.P[6]],
[camera.P[8], camera.P[9], camera.P[10]]])
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(object_points, image_points,
cv.fromarray(camera_matrix),
dist_coeffs, rot, trans)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (
numpy.asmatrix(object_points).T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points - reprojected.T
reprojection_rms = numpy.sqrt(
numpy.sum(numpy.array(reprojection_errors)**2) /
numpy.product(reprojection_errors.shape))
# Print the results
print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (
linearity_rms, reprojection_rms)
else:
print 'no chessboard'
def __initialiseUndistortMap__(self):
#print "CameraAccessor - initialiseUndistortMap begin"
testImage = cv.QueryFrame(self.camera)
#print "imageType = ", type(testImage)
self.photoSize = cv.GetSize(testImage)
self.mapx = cv.CreateImage(self.photoSize, 32, 1)
self.mapy = cv.CreateImage(self.photoSize, 32, 1)
cv.InitUndistortMap(cv.fromarray(self.intrinsecParameters), cv.fromarray(self.distortionParameter), self.mapx, self.mapy)
def execute(signature1, signature2, data):
codewords = data["codewords"]
fsig1 = cv.fromarray(numpy.asarray([[count, ] + list(word)
for count, word in zip(signature1, codewords)], dtype=numpy.float32))
fsig2 = cv.fromarray(numpy.asarray([[count, ] + list(word)
for count, word in zip(signature2, codewords)], dtype=numpy.float32))
distance = cv.CalcEMD2(fsig1, fsig2, cv.CV_DIST_L2)
return distance
def Test(img0, img1, H0, H1):
buff = cv.LoadImage('buff.jpg')
H0 = cv.fromarray(H0.T.copy())
cv.WarpPerspective(img0, buff, H0)
cv.SaveImage("Results/warpBack0.jpg", buff)
buff = cv.LoadImage('buff.jpg')
H1 = cv.fromarray(H1.T.copy())
cv.WarpPerspective(img1, buff, H1)
cv.SaveImage("Results/warpBack1.jpg",buff)
def canny_it(self, iteration):
print "24 canny_it"
#called by 27#
#called by 24 (iterative)#
#called by 23#
if self.save_images:
# save raw image of chess board
file_name = self.image_dir + "chess_board_" + str(
iteration) + ".jpg"
self.cv_image = self.cv_image
cv.SaveImage(file_name, cv.fromarray(self.cv_image))
# create an empty image variable, the same dimensions as our camera feed.
gray = cv.CreateImage((cv.GetSize(cv.fromarray(self.cv_image))), 8, 1)
# convert the image to a grayscale image
cv.CvtColor(cv.fromarray(self.cv_image), gray, cv.CV_BGR2GRAY)
# display image on head monitor
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 1)
position = (30, 60)
cv.PutText(cv.fromarray(self.cv_image), "Buscando Tablero", position,
font, self.white)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
# create a canny edge detection map of the greyscale
cv.Canny(gray, self.canny, self.canny_low, self.canny_high, 3)
# display the canny transformation
cv.ShowImage("Canny Edge Detection", self.canny)
if self.save_images:
# save Canny image of chess board
file_name = self.image_dir + "canny_board_" + str(
iteration) + ".jpg"
cv.SaveImage(file_name, self.canny)
# flood fill edge of image to leave only objects "go to 9"
self.flood_fill_edge(self.canny)
#//back from 9, "go to 8"
chess_board_centre = self.look_for_chess_board(self.canny)
#//back from 8
# 3ms wait
cv.WaitKey(3)
while chess_board_centre[0] == 0:
if random.random() > 0.6:
self.baxter_ik_move(self.limb, self.dither())
#"go to 24" (iterate)
chess_board_centre = self.canny_it(iteration)
#//back from 24
return chess_board_centre
def get_homography(self):
# set up source points (model points)
srcPoints = cv.fromarray(np.matrix([[63, 343], [537, 367], [550, 137], [78, 123]], dtype=float))
# set up destination points (observed object points)
# dstPoints = cv.fromarray(np.matrix([[120,285],[420,359],[455,186],[228,143]], dtype=float))
dstPoints = cv.fromarray(np.matrix([[22, 383], [608, 385], [541, 187], [100, 196]], dtype=float))
# compute homography
H = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(srcPoints, dstPoints, H) # def. setting is [method=0,ransacReprojThreshold=3.0,status=None]
return H
def rgb2gray(I):
if len(I.shape) < 3:
return I
else:
Ichn1 = I[:, :, 0]
Icv = cv.fromarray(np.copy(I))
Ichn1cv = cv.fromarray(np.copy(Ichn1))
cv.CvtColor(Icv, Ichn1cv, cv.CV_RGB2GRAY)
Iout = np.asarray(Ichn1cv)
return Iout
def _ransac(self, fkp, tkp):
H = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(cv.fromarray(fkp), cv.fromarray(tkp), H,
method=cv.CV_RANSAC, ransacReprojThreshold=5)
H = np.array(H)
sx = abs(H[0][0])
sy = abs(H[1][1])
scale_ratio = min(sx, sy) / max(sx, sy)
nb_inliners = self._get_nb_inliers(H, fkp, tkp)
return nb_inliners, scale_ratio
def histEMD(hist1, hist2, hist1weights, hist2weights):
a64 = cv.fromarray(np.hstack((hist1weights, hist1)).copy())
a32 = cv.CreateMat(a64.rows, a64.cols, cv.CV_32FC1)
cv.Convert(a64, a32)
b64 = cv.fromarray(np.hstack((hist2weights, hist2)).copy())
b32 = cv.CreateMat(b64.rows, b64.cols, cv.CV_32FC1)
cv.Convert(b64, b32)
return cv.CalcEMD2(a32, b32, cv.CV_DIST_L2)
def histEMD(hist1, hist2, hist1weights, hist2weights):
a64 = cv.fromarray(np.hstack((hist1weights, hist1)).copy())
a32 = cv.CreateMat(a64.rows, a64.cols, cv.CV_32FC1)
cv.Convert(a64, a32)
b64 = cv.fromarray(np.hstack((hist2weights, hist2)).copy())
b32 = cv.CreateMat(b64.rows, b64.cols, cv.CV_32FC1)
cv.Convert(b64, b32)
return cv.CalcEMD2(a32,b32,cv.CV_DIST_L2)
def observation(self, meas):
meas = np.float32([meas])
if not self.initialized:
cv.Copy(cv.fromarray(meas.T.copy()), self.kf.state_post)
cv.Copy(cv.fromarray(meas.T.copy()), self.kf.state_pre)
self.initialized = True
if self.kf.CP == 0:
cv.KalmanPredict(self.kf)
corrected = np.asarray(
cv.KalmanCorrect(self.kf, cv.fromarray(meas.T.copy())))
return corrected.T[0].copy()
def fuse(file_path):
img = cv2.imread(file_path, 1)
cv.ShowImage('origin', cv.fromarray(img))
cv.WaitKey(0)
for i in xrange(1):
kernel = np.ones((20,20), np.float32)/2500
img = cv2.filter2D(img, -1, kernel)
cv.ShowImage('origin', cv.fromarray(img))
cv.WaitKey(0)
def achromatic_mask(image):
copy = image.copy()
(cols, rows) = cv.GetSize(cv.fromarray(copy))
hls = cv2.cvtColor(copy, cv2.COLOR_BGR2HLS)
(hue, lightness, saturation) = cv2.split(hls)
inverted_mask = cv2.inRange(hue, np.array(0), np.array(0))
#print sum(sum(inverted_mask)) / 255
mask = cv2.bitwise_not(inverted_mask)
cv.Set(cv.fromarray(copy), (0, 0, 0), cv.fromarray(inverted_mask))
cv2.imshow('achromatic mask', copy)
return mask, inverted_mask
def main():
I_np = shared.standardImread(Ipath, flatten=True)
I_np = shared.prepOpenCV(I_np)
cv_imgBAD = cv.fromarray(np.copy(I_np))
cv.SaveImage("party_CV_BAD.png", cv_imgBAD)
cv_imgGOOD = cv.fromarray(np.copy(I_np) * 255.0)
cv.SaveImage("party_CV_GOOD.png", cv_imgGOOD)
print "Done."
def distance3(img, imgpath2):
""" NCC score between img1, img2. """
imgCv = cv.fromarray(np.copy(img.astype(np.float32)))
img2 = shared.standardImread(imgpath2, flatten=True)
bb2 = bb_map.get(imgpath2, None)
if bb2 != None:
img2 = img2[bb2[0]:bb2[2], bb2[2]:bb2[3]]
img2Cv = cv.fromarray(np.copy(img2.astype(np.float32)))
outCv = cv.CreateMat(imgCv.height - img2Cv.height + 1,
imgCv.width - img2Cv.width + 1, imgCv.type)
cv.MatchTemplate(imgCv, img2Cv, outCv, cv.CV_TM_CCOEFF_NORMED)
return outCv.max()
def logpolar(img, center=None, mag=1):
des = N.empty_like(img)
if center is None:
center = N.divide(img.shape, 2)
# cv.fromarray: array can be 2D or 3D only
cimg = cv.fromarray(img)
cdes = cv.fromarray(des)
cv.LogPolar(cimg, cdes, tuple(center), mag)#, cv.CV_WARP_FILL_OUTLIERS)
return N.array(cdes)
