def detect(self, image):
corners_x = self.chess_size[0]
corners_y = self.chess_size[1]
#Here, we'll actually call the openCV detector
found, corners = cv.FindChessboardCorners(
image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if found:
board_corners = (corners[0], corners[corners_x - 1],
corners[(corners_y - 1) * corners_x],
corners[len(corners) - 1])
#find the perimeter of the checkerboard
perimeter = 0.0
for i in range(len(board_corners)):
next = (i + 1) % 4
xdiff = board_corners[i][0] - board_corners[next][0]
ydiff = board_corners[i][1] - board_corners[next][1]
perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
#estimate the square size in pixels
square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2)
radius = int(square_size * 0.5 + 0.5)
corners = cv.FindCornerSubPix(
image, corners, (radius, radius), (-1, -1),
(cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
#uncomment to debug chessboard detection
#cv.DrawChessboardCorners(image, (corners_x, corners_y), corners, 1)
#cv.NamedWindow("image")
#cv.ShowImage("image", image)
#cv.WaitKey(600)
#we'll also generate the object points if the user has specified spacing
object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1)
for y in range(corners_y):
for x in range(corners_x):
cv.SetReal2D(object_points, 0, y * corners_x + x,
x * self.dim)
cv.SetReal2D(object_points, 1, y * corners_x + x,
y * self.dim)
cv.SetReal2D(object_points, 2, y * corners_x + x, 0.0)
#not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1)
for i in range(corners_x * corners_y):
cv.SetReal2D(corners_cv, 0, i, corners[i][0])
cv.SetReal2D(corners_cv, 1, i, corners[i][1])
return (corners_cv, object_points)
else:
#cv.NamedWindow("image_scaled")
#cv.ShowImage("image_scaled", image_scaled)
#cv.WaitKey(600)
rospy.logwarn("Didn't find checkerboard")
return (None, None)
return
def detect(self, image):
#resize the image base on the scaling parameters we've been configured with
scaled_width = int(.5 + image.width * self.width_scaling)
scaled_height = int(.5 + image.height * self.height_scaling)
#in cvMat its row, col so height comes before width
image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image))
cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR)
#Here, we'll actually call the openCV detector
found, corners = cv.FindChessboardCorners(image_scaled, (self.corners_x, self.corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if found:
board_corners = self.get_board_corners(corners)
#find the perimeter of the checkerboard
perimeter = 0.0
for i in range(len(board_corners)):
next = (i + 1) % 4
xdiff = board_corners[i][0] - board_corners[next][0]
ydiff = board_corners[i][1] - board_corners[next][1]
perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
#estimate the square size in pixels
square_size = perimeter / ((self.corners_x - 1 + self.corners_y - 1) * 2)
radius = int(square_size * 0.5 + 0.5)
corners = cv.FindCornerSubPix(image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
if self.display:
#uncomment to debug chessboard detection
cv.DrawChessboardCorners(image_scaled, (self.corners_x, self.corners_y), corners, 1)
cv.NamedWindow("image_scaled")
cv.ShowImage("image_scaled", image_scaled)
cv.WaitKey(5)
object_points = None
#we'll also generate the object points if the user has specified spacing
if self.spacing_x != None and self.spacing_y != None:
object_points = cv.CreateMat(3, self.corners_x * self.corners_y, cv.CV_32FC1)
for y in range(self.corners_y):
for x in range(self.corners_x):
cv.SetReal2D(object_points, 0, y*self.corners_x + x, x * self.spacing_x)
cv.SetReal2D(object_points, 1, y*self.corners_x + x, y * self.spacing_y)
cv.SetReal2D(object_points, 2, y*self.corners_x + x, 0.0)
#not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
corners_cv = cv.CreateMat(2, self.corners_x * self.corners_y, cv.CV_32FC1)
for i in range(self.corners_x * self.corners_y):
cv.SetReal2D(corners_cv, 0, i, corners[i][0])
cv.SetReal2D(corners_cv, 1, i, corners[i][1])
return (corners_cv, object_points)
else:
rospy.logdebug("Didn't find checkerboard")
return (None, None)
def SetReal2DAround(source_image, loc, size, value):
u'''locで指定された画素を中心に縦横sizeの範囲にある画素をvalueにする'''
assert size >= 1 and size % 2 == 1, u'sizeは1以上の奇数である必要があります'
rows = xrange(loc[1] - (size - 1) / 2, loc[1] + (size - 1) / 2 + 1)
cols = xrange(loc[0] - (size - 1) / 2, loc[0] + (size - 1) / 2 + 1)
for row in rows:
if 0 <= row < source_image.height:
for col in cols:
if 0 <= col < source_image.width:
cv.SetReal2D(source_image, row, col, value)
def normalizeImage(frame):
redChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
greenChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
blueChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
redAvg = cv.CreateImage(cv.GetSize(frame), 8, 1)
greenAvg = cv.CreateImage(cv.GetSize(frame), 8, 1)
blueAvg = cv.CreateImage(cv.GetSize(frame), 8, 1)
resImg = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.Split(frame, blueChannel, greenChannel, redChannel, None)
hsvImg = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(frame, hsvImg, cv.CV_BGR2HSV)
hueChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
satChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
valChannel = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.Split(hsvImg, hueChannel, satChannel, valChannel, None)
for y in range(0, frame.height):
for x in range(0, frame.width):
redValue = cv.GetReal2D(redChannel, y, x)
greenValue = cv.GetReal2D(greenChannel, y, x)
blueValue = cv.GetReal2D(blueChannel, y, x)
sum = redValue + greenValue + blueValue + 0.0
if (sum < 1.0):
sum = 1.0
cv.SetReal2D(redAvg, y, x, redValue / sum * 255)
cv.SetReal2D(greenAvg, y, x, greenValue / sum * 255)
cv.SetReal2D(blueAvg, y, x, blueValue / sum * 255)
cv.Merge(blueAvg, greenAvg, redAvg, None, resImg)
del redChannel
del greenChannel
del blueChannel
del redAvg
del greenAvg
del blueAvg
return (resImg)
def saturate(frame):
cv.CvtColor(frame, frame, cv.CV_BGR2HSV)
hue = cv.CreateImage(cv.GetSize(frame), 8, 1)
sat = cv.CreateImage(cv.GetSize(frame), 8, 1)
val = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.Split(frame, hue, sat, val, None)
for y in range(0, frame.height):
for x in range(0, frame.width):
saturation = cv.GetReal2D(sat, y, x)
if (saturation < 200):
saturation = saturation + 50
cv.SetReal2D(sat, y, x, saturation)
cv.Merge(hue, sat, val, None, frame)
del hue
del sat
del val
cv.CvtColor(frame, frame, cv.CV_HSV2BGR)
return (frame)
def solve_linear(v1, v2, p1, p2):
A = cv.CreateMat(2, 2, cv.CV_32FC1)
B = cv.CreateMat(2, 1, cv.CV_32FC1)
X = cv.CreateMat(2, 1, cv.CV_32FC1)
cv.SetReal2D(A, 0, 0, v1[0])
cv.SetReal2D(A, 1, 0, v1[1])
cv.SetReal2D(A, 0, 1, -v2[0])
cv.SetReal2D(A, 1, 1, -v2[1])
cv.SetReal2D(B, 0, 0, p2[0] - p1[0])
cv.SetReal2D(B, 1, 0, p2[1] - p1[1])
cv.Solve(A, B, X)
p = cv.Get2D(X, 0, 0)
q = cv.Get2D(X, 1, 0)
#print 'solving p1=',p1,'v1=',v1,'p2=',p2,'v2=',v2,'result=',(p[0],q[0])
return (p[0], q[0])
def extrinsic(frame):
frame.lower()
rvec = cv.CreateMat(1,3,cv.CV_32FC1)
cv.SetZero(rvec)
rvec_0 = rospy.get_param('/camera_' + frame + '_rvec_0')
rvec_1 = rospy.get_param('/camera_' + frame + '_rvec_1')
rvec_2 = rospy.get_param('/camera_' + frame + '_rvec_2')
cv.SetReal2D(rvec,0,0,rvec_0)
cv.SetReal2D(rvec,0,1,rvec_1)
cv.SetReal2D(rvec,0,2,rvec_2)
tvec = cv.CreateMat(1,3,cv.CV_32FC1)
cv.SetZero(tvec)
tvec_0 = rospy.get_param('/camera_' + frame + '_tvec_0')
tvec_1 = rospy.get_param('/camera_' + frame + '_tvec_1')
tvec_2 = rospy.get_param('/camera_' + frame + '_tvec_2')
cv.SetReal2D(tvec,0,0,tvec_0)
cv.SetReal2D(tvec,0,1,tvec_1)
cv.SetReal2D(tvec,0,2,tvec_2)
return rvec, tvec
