class ImageCbDetectorNode:
def __init__(self):
self.display = rospy.get_param('~display', True)
self.corners_x = rospy.get_param('~corners_x', 5)
self.corners_y = rospy.get_param('~corners_y', 5)
self.spacing_x = rospy.get_param('~spacing_x', None)
self.spacing_y = rospy.get_param('~spacing_y', None)
self.width_scaling = rospy.get_param('~width_scaling', 1)
self.height_scaling = rospy.get_param('~height_scaling', 1)
self.im_cb_detector = ImageCbDetector(self.display,self.corners_x, self.corners_y, self.spacing_x,
self.spacing_y, self.width_scaling, self.height_scaling)
self.image_sub = rospy.Subscriber("~image", Image, self.callback)
self.cb_pub = rospy.Publisher("~checkerboard", Checkerboard)
self.bridge = CvBridge()
def callback(self, ros_image):
#we need to convert the ros image to an opencv image
try:
image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
except CvBridgeError, e:
rospy.logerror("Error importing image %s" % e)
return
corners, model = self.im_cb_detector.detect(image)
if corners:
cb = Checkerboard()
cb.header = ros_image.header
cb.num_x = self.corners_x
cb.num_y = self.corners_y
for i in range(self.corners_x * self.corners_y):
cb.points.append(Point(cv.GetReal2D(corners,0,i),cv.GetReal2D(corners,1,i),0))
if model:
cb.model.append(Point(cv.GetReal2D(model,0,i),cv.GetReal2D(model,1,i),cv.GetReal2D(model,2,i)))
self.cb_pub.publish(cb)
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)
velx = cv.CreateMat(rows, cols, cv.CV_32FC1)
vely = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(velx)
cv.SetZero(vely)
cv.CalcOpticalFlowHS(src_im1, src_im2, 0, velx, vely, 100.0,
(cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 64, 0.01))
#cv.CalcOpticalFlowLK(src_im1, src_im2, (10,10), velx, vely)
print velx
print vely
for i in range(0, (cols - 1), 5):
for j in range(0, (rows - 1), 5):
dx = cv.GetReal2D(velx, j, i)
dy = cv.GetReal2D(vely, j, i)
cv.Line(dst_im1, (i, j), (int(i + dx), int(j + dy)),
cv.CV_RGB(255, 0, 0), 1, cv.CV_AA, 0)
cv.NamedWindow("w", cv.CV_WINDOW_AUTOSIZE)
cv.ShowImage("w", dst_im1)
cv.WaitKey()
##using Lucas Kanade
if args.algorithm == 'LK':
dst_img = cv.LoadImage(args.im2, cv.CV_LOAD_IMAGE_COLOR)
eign_img = cv.CreateImage(cv.GetSize(src_im1), cv.IPL_DEPTH_32F, 1)
temp_img = cv.CreateImage(cv.GetSize(src_im1), cv.IPL_DEPTH_32F, 1)
features = cv.GoodFeaturesToTrack(src_im1, eign_img, temp_img, 5000, 0.1,
10, None, True)
cols = inputImageFirst.width
rows = inputImageFirst.height
print cols, rows
velx = cv.CreateMat(rows, cols, cv.CV_32FC1)
vely = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(velx)
cv.SetZero(vely)
cv.CalcOpticalFlowBM(inputImageFirst, inputImageSecond, (1, 1), (1, 1), (1, 1),
0, velx, vely)
f = open('./F/0.5/shuibo_05_BM(x1,y1,x1,y2).txt', 'w') #将光流保存到txt文件中
count = 0
for i in range(0, cols, FLOWSKIP):
for j in range(0, rows, FLOWSKIP):
dx = int(cv.GetReal2D(velx, j, i))
dy = int(cv.GetReal2D(vely, j, i))
cv.Line(desImageHS, (i, j), (i + dx, j + dy), (0, 0, 255), 1, cv.CV_AA,
0)
f.writelines(
[str(i), ' ',
str(j), ' ',
str(i + dx), ' ',
str(j + dy), '\n'])
# count+=1
# print count
f.close()
cv.SaveImage("resultHS.png", desImageHS)
cv.NamedWindow("Optical flow HS")
def find_checkerboard_service(self, req):
poses = []
min_x = self.ros_image.width
min_y = self.ros_image.height
max_x = 0
max_y = 0
for i in range(10):
rospy.sleep(0.5)
#copy the image over
with self.mutex:
image = self.ros_image
result = self.find_checkerboard_pose(image, req.corners_x,
req.corners_y, req.spacing_x,
req.spacing_y,
self.width_scaling,
self.height_scaling)
if result is None:
rospy.logerr("Couldn't find a checkerboard")
continue
p, corners = result
poses.append(p)
for j in range(corners.cols):
x = cv.GetReal2D(corners, 0, j)
y = cv.GetReal2D(corners, 1, j)
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
# convert all poses to twists
twists = []
for p in poses:
twists.append(
PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
# get average twist
twist_res = PyKDL.Twist()
PyKDL.SetToZero(twist_res)
for t in twists:
for i in range(3):
twist_res.vel[i] += t.vel[i] / len(poses)
twist_res.rot[i] += t.rot[i] / len(poses)
# get noise
noise_rot = 0
noise_vel = 0
for t in twists:
n_vel = (t.vel - twist_res.vel).Norm()
n_rot = (t.rot - twist_res.rot).Norm()
if n_vel > noise_vel:
noise_vel = n_vel
if n_rot > noise_rot:
noise_rot = n_rot
# set service resul
pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
pose_msg = PoseStamped()
pose_msg.header = poses[0].header
pose_msg.pose = posemath.toMsg(pose_res)
return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y,
max_y, noise_vel, noise_rot)
def cut(disparity, image, threshold):
for i in range(0, image.height):
for j in range(0, image.width):
# keep closer object
if cv.GetReal2D(disparity,i,j) > threshold:
cv.Set2D(disparity,i,j,cv.Get2D(image,i,j))
