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