def handle_msg(self, msg):
"""
Detects the calibration target and, if found and provides enough new information,
adds it to the sample database.
Returns a MonoDrawable message with the display image and progress info.
"""
gray = self.mkgray(msg)
linear_error = -1
# Get display-image-to-be (scrib) and detection of the calibration target
scrib_mono, corners, downsampled_corners, board, (x_scale, y_scale) = self.downsample_and_detect(gray)
# make sure scrib has 3 channels for display
scrib = cv.CreateMat(scrib_mono.rows, scrib_mono.cols, cv.CV_8UC3)
if self.calibrated:
# Show rectified image
# TODO Pull out downsampling code into function
gray_remap = self.remap(gray)
gray_rect = gray_remap
if x_scale != 1.0 or y_scale != 1.0:
gray_rect = cv.CreateMat(scrib.rows, scrib.cols, cv.CV_8UC1)
cv.Resize(gray_remap, gray_rect)
cv.CvtColor(gray_rect, scrib, cv.CV_GRAY2BGR)
if corners:
# Report linear error
src = self.mk_image_points([ (corners, board) ])
undistorted = list(cvmat_iterator(self.undistort_points(src)))
linear_error = self.linear_error(undistorted, board)
# Draw rectified corners
scrib_src = [(x/x_scale, y/y_scale) for (x,y) in undistorted]
cv.DrawChessboardCorners(scrib, (board.n_cols, board.n_rows), scrib_src, True)
else:
cv.CvtColor(scrib_mono, scrib, cv.CV_GRAY2BGR)
if corners:
# Draw (potentially downsampled) corners onto display image
src = self.mk_image_points([ (downsampled_corners, board) ])
cv.DrawChessboardCorners(scrib, (board.n_cols, board.n_rows), cvmat_iterator(src), True)
# Add sample to database only if it's sufficiently different from any previous sample.
params = self.get_parameters(corners, board, cv.GetSize(gray))
if self.is_good_sample(params):
self.db.append((params, gray))
self.good_corners.append((corners, board))
print "*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)
rv = MonoDrawable()
rv.scrib = scrib
rv.params = self.compute_goodenough()
rv.linear_error = linear_error
return rv
def image_filter(self, cv_image, info, copy=None):
board_sz = (self.cols, self.rows)
(found, corners) = cv.FindChessboardCorners(
cv_image, board_sz,
(cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS))
cv.DrawChessboardCorners(cv_image, (self.cols, self.rows), corners, 1)
return cv_image
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 process(self, gui):
"""Process file, find corners on chessboard and keep points
"""
gui.setMessage("Analyzing movie... get frame count")
#open capture file
capture = cv.CaptureFromFile(self.filename)
frame = cv.QueryFrame(capture)
#count... I know it sucks
numframes=1
while frame:
frame = cv.QueryFrame(capture)
numframes +=1
step = int(numframes/Calibration.NUMPOINT)
capture = cv.CaptureFromFile(self.filename)
frame = cv.QueryFrame(capture) #grab a frame to get some information
self.framesize = (frame.width, frame.height)
gray = cv.CreateImage((frame.width,frame.height), 8, 1)
points = []
nframe = 0
f=0
cv.NamedWindow("ChessBoard",cv.CV_WINDOW_NORMAL)
gui.setMessage("Analyzing movie... find chessCorner for %d frames" % Calibration.NUMPOINT)
while frame:
f+=1
#find corners
state,found = cv.FindChessboardCorners(frame, self.chessSize, flags=cv.CV_CALIB_CB_FILTER_QUADS)
if state==1:
#affine search
cv.CvtColor( frame, gray, cv.CV_BGR2GRAY )
found = cv.FindCornerSubPix(gray, found, (11,11), (-1,-1), (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 30, 0.1))
#draw corners on image
cv.DrawChessboardCorners(frame, self.chessSize, found, state)
#compute points
for x,y in found:
points.append((x,y))
nframe+=1
cv.ResizeWindow("ChessBoard",640,480)
cv.ShowImage("ChessBoard",frame)
cv.WaitKey(4)
frame = cv.QueryFrame(capture) #grab a frame to get some information
#skip frames to get only NUMPOINT of point to calculate
while f%step != 0 and frame:
f+=1
frame = cv.QueryFrame(capture)
self.points = points
self.nframe = nframe
gui.setMessage("Analyze end, %d points found" % len(self.points))
def draw_chessboard(img, chessboard, ncol=5, nrow=4):
'''
draws the chessboard detection from find_chessboard on the image
'''
# convert image msg to cv
if (type(img) == sensor_msgs.Image):
img = msg2cvmat(img);
chessboard = tuple(map(tuple, chessboard));
cv.DrawChessboardCorners(img, (ncol, nrow), chessboard, True);
return img;
def detect(image):
image_size = cv.GetSize(image)
# create grayscale version
grayscale = cv.CreateImage(image_size, 8, 1)
cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY)
storage = cv.CreateMemStorage(0)
im = cv.CreateImage(image_size, 8, 3)
status, corners = cv.FindChessboardCorners(grayscale, (dim, dim))
if status:
cv.DrawChessboardCorners(image, (dim, dim), corners, status)
is_x = [p[0] for p in corners]
is_y = [p[1] for p in corners]
return is_x, is_y
return [], []
def on_enter_frame(frame):
global have_corners, corners, rowcols, intrinsics, distortion
w, h = cv.GetSize(frame)
img = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
cv.CvtColor(frame, img, cv.CV_RGB2GRAY)
have_corners, corners = cv.FindChessboardCorners(img, rowcols)
if have_corners:
cv.DrawChessboardCorners(frame, rowcols, corners, True)
if intrinsics and distortion:
new = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 3)
cv.Undistort2(frame, new, intrinsics, distortion)
cv.ShowImage('Calibrated', new)
cv.ShowImage('Frame', frame)
def main():
print "This is the capture utility for the FreenetFusion calibration."
print "1 - Place the calibration template in front of the Kinect sensor"
print "2 - Make sure the calibration template is detected (you will see a colored grid)"
print "3 - Press SPACE. DO NOT MOVE the calibration template"
print "4 - Repeat from 1 with many (~10-20) different poses of the calibration pattern"
print "5 - Press ESC when done\n"
cv.NamedWindow('Capture')
index = 0
while True:
img = gray2color(
freenect.sync_get_video(format=freenect.VIDEO_IR_8BIT)[0])
img_cv = prepare_cv(img)
detected, corners = cv.FindChessboardCorners(img_cv, (8, 6))
if detected:
cv.DrawChessboardCorners(img_cv, (8, 6), corners, 1)
cv.ShowImage("Capture", img_cv)
key = cv.WaitKey(1)
if key == 27:
break
if key != 32:
continue
if not detected:
print "Chessboard template not detected. Please, retry.\n"
continue
print "Capturing and detecting the chessboard template in the RGB image..."
new_index = capture_pair(index, img)
if new_index == index:
print "The calibration template was not found in the RGB image. Please, retry.\n"
continue
index = new_index
print "Success! Images have been saved. You have taken %d correct image pairs." % index
if index < 10:
print "At least 10 images are required for a good calibration."
print "Please, change the position of the calibration template and repeat.\n"
def get_image():
im = cv.QueryFrame(camera)
# cv.Flip(im, flipMode=1) # mirror effect
success, corners = cv.FindChessboardCorners(im, PAT_SIZE, findFlags)
if success:
cv.DrawChessboardCorners(im, PAT_SIZE, corners, success)
cv.PutText(im, "Found: (%.1f, %.1f)" % corners[0], infoOrigin,
infoFont, (0, 255, 0))
cam_matrix = cv.CreateMat(3, 3, cv.CV_32F)
dist_coeff = cv.CreateMat(1, 4, cv.CV_32F)
rvecs = cv.CreateMat(1, 9, cv.CV_32F)
tvecs = cv.CreateMat(1, 3, cv.CV_32F)
pointArr = numpy.array([(x, y, 0) for y in xrange(PAT_SIZE[1]) for x in xrange(PAT_SIZE[0])], numpy.float32)
objectPoints = cv.fromarray(pointArr)
imgPointArr = numpy.array(corners, numpy.float32)
imagePoints = cv.fromarray(imgPointArr)
pointCounts = cv.CreateMat(1, 1, cv.CV_32S)
pointCounts[0, 0] = PAT_SIZE[0] * PAT_SIZE[1]
# Rodrigues version:
rvecs3 = cv.CreateMat(1, 3, cv.CV_32F)
cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, IMG_SIZE, cam_matrix, dist_coeff, rvecs3, tvecs)
rmat3 = cv.CreateMat(3, 3, cv.CV_32F)
cv.Rodrigues2(rvecs3, rmat3)
# end Rodrigues version
#cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, IMG_SIZE, cam_matrix, dist_coeff, rvecs, tvecs)
#rmat = numpy.asarray(rvecs).reshape((3, 3), order='C')
#print "RVecs:"
#print rmat
print "TVecs:", numpy.asarray(tvecs)
# 3. column of R == rotated z versor, angle toward x; z=(2, 2), x=(0, 2)
yaw = math.atan2(rmat3[0, 2], rmat3[2, 2]) * 180 / math.pi
# rotated z versor, height y=(1, 2) to length==1
pitch = math.asin(rmat3[1, 2]) * 180 / math.pi
# 1. column of R = rotated x versor, height y = (1, 0) to length==1
roll = math.asin(rmat3[1, 0]) * 180 / math.pi
print "Yaw %5.2f, Pitch %5.2f, Roll %5.2f" % (yaw, pitch, roll)
#import pdb; pdb.set_trace()
print '-'*40
else:
cv.PutText(im, "Not found.", infoOrigin, infoFont, (0, 255, 0))
return cv2pygame(im)
#!/usr/bin/python
import cv
import sys
import urllib2
if __name__ == "__main__":
cv.NamedWindow("win")
if len(sys.argv) > 1:
filename = sys.argv[1]
im = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_GRAYSCALE)
im3 = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_COLOR)
else:
url = 'https://code.ros.org/svn/opencv/trunk/opencv/samples/c/left01.jpg'
filedata = urllib2.urlopen(url).read()
imagefiledata = cv.CreateMatHeader(1, len(filedata), cv.CV_8UC1)
cv.SetData(imagefiledata, filedata, len(filedata))
im = cv.DecodeImageM(imagefiledata, cv.CV_LOAD_IMAGE_GRAYSCALE)
im3 = cv.DecodeImageM(imagefiledata, cv.CV_LOAD_IMAGE_COLOR)
chessboard_dim = ( 9, 6 )
found_all, corners = cv.FindChessboardCorners( im, chessboard_dim )
print found_all, len(corners)
cv.DrawChessboardCorners( im3, chessboard_dim, corners, found_all )
cv.ShowImage("win", im3);
cv.WaitKey()
def annotate_image(cv_im, mech_info_dict, dir):
#cv_im = cv.LoadImage(sys.argv[1], cv.CV_LOAD_IMAGE_GRAYSCALE)
size = cv.GetSize(cv_im)
print 'Image size:', size[0], size[1] # col, row
wnd = 'Image Annotate'
cv.NamedWindow(wnd, cv.CV_WINDOW_AUTOSIZE)
disp_im = cv.CloneImage(cv_im)
new_size = (size[0] / 2, size[1] / 2)
scale_factor = 1
checker_origin_height = mech_info_dict['height']
# chesscoard corners mat
cb_dims = (5, 8) # checkerboard dims. (x, y)
sq_sz = 19 # size of checkerboard in real units.
cb_offset = 500, 500
cb_coords = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1)
n = 0
for r in range(cb_dims[1]):
for c in range(cb_dims[0]):
cb_coords[0, n] = c * sq_sz + cb_offset[0] # x coord
cb_coords[1, n] = r * sq_sz + cb_offset[1] # y coord
n += 1
clicked_list = []
recreate_image = False
mechanism_calc_state = 0
mechanism_calc_dict = {}
while True:
for p in clicked_list:
x, y = p[0] * scale_factor, p[1] * scale_factor
cv.Circle(disp_im, (x, y), 3, (255, 0, 0))
cv.ShowImage(wnd, disp_im)
k = cv.WaitKey(10)
k = k % 256
if k == 27:
# ESC
break
elif k == ord('='):
# + key without the shift
scale_factor = scale_factor * 1.2
recreate_image = True
elif k == ord('-'):
# - key
scale_factor = scale_factor / 1.2
recreate_image = True
elif k == ord('c'):
# find chessboard corners.
succ, corners = cv.FindChessboardCorners(disp_im, cb_dims)
if succ == 0:
print 'Chessboard detection FAILED.'
else:
# chessboard detection was successful
cv.DrawChessboardCorners(disp_im, cb_dims, corners, succ)
cb_im = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1)
corners_mat = np.array(corners).T
n = 0
for r in range(cb_dims[1]):
for c in range(cb_dims[0]):
cb_im[0, n] = corners_mat[0, n] # x coord
cb_im[1, n] = corners_mat[1, n] # y coord
n += 1
H = cv.CreateMat(3, 3, cv.CV_64FC1)
H1 = cv.FindHomography(cb_im, cb_coords, H)
Hnp = np.reshape(np.fromstring(H1.tostring()), (3, 3))
print 'Homography:'
print Hnp
d = cv.CloneImage(disp_im)
cv.WarpPerspective(d, disp_im, H1, cv.CV_WARP_FILL_OUTLIERS)
cv_im = cv.CloneImage(disp_im)
elif k == ord('1'):
# calculate width of the mechanism
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on two endpoints to mark the width of the mechanism'
mechanism_calc_state = 1
elif k == ord('2'):
# distance of handle from the hinge
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on handle and hinge to compute distance of handle from hinge.'
mechanism_calc_state = 2
elif k == ord('3'):
# height of the handle above the ground
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on handle top and bottom to compute height of handle above the ground.'
mechanism_calc_state = 3
elif k == ord('4'):
# top and bottom edge of the mechanism
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on top and bottom edges of the mechanism.'
mechanism_calc_state = 4
elif k == ord('d'):
# display the calculated values
print 'mechanism_calc_dict:', mechanism_calc_dict
print 'mech_info_dict:', mech_info_dict
elif k == ord('s'):
# save the pkl
ut.save_pickle(mechanism_calc_dict,
dir + '/mechanism_calc_dict.pkl')
print 'Saved the pickle'
#elif k != -1:
elif k != 255:
print 'k:', k
if recreate_image:
new_size = (int(size[0] * scale_factor),
int(size[1] * scale_factor))
disp_im = cv.CreateImage(new_size, cv_im.depth, cv_im.nChannels)
cv.Resize(cv_im, disp_im)
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = False
if len(clicked_list) == 2:
if mechanism_calc_state == 1:
w = abs(clicked_list[0][0] - clicked_list[1][0])
print 'Width in mm:', w
mechanism_calc_dict['mech_width'] = w / 1000.
if mechanism_calc_state == 2:
w = abs(clicked_list[0][0] - clicked_list[1][0])
print 'Width in mm:', w
mechanism_calc_dict['handle_hinge_dist'] = w / 1000.
if mechanism_calc_state == 3:
p1, p2 = clicked_list[0], clicked_list[1]
h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height
h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height
mechanism_calc_dict['handle_top'] = max(h1, h2)
mechanism_calc_dict['handle_bottom'] = min(h1, h2)
if mechanism_calc_state == 4:
p1, p2 = clicked_list[0], clicked_list[1]
h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height
h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height
mechanism_calc_dict['mechanism_top'] = max(h1, h2)
mechanism_calc_dict['mechanism_bottom'] = min(h1, h2)
mechanism_calc_state = 0
image = cv.QueryFrame(cam)
gray_image = cv.CreateImage(cv.GetSize(image), 8, 1)
while (successes < num_boards):
frame += 1
if (frame % num_framestep == 0):
corners = cv.FindChessboardCorners(
image, board_size,
cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)
corners = corners[1]
cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY)
cv.FindCornerSubPix(
gray_image, corners, (11, 11), (0, 0),
(cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1))
if (len(corners) > 1):
cv.DrawChessboardCorners(image, board_size, corners, 1)
if (len(corners) == board_total):
cv.ShowImage("Snapshot", image)
step = successes * board_total
i = step
for j in range(board_total):
cv.Set2D(image_points, i, 0, corners[j][0])
cv.Set2D(image_points, i, 1, corners[j][1])
cv.Set2D(object_points, i, 0, float(j) / num_horizontal)
cv.Set2D(object_points, i, 1, float(j % num_horizontal))
cv.Set2D(object_points, i, 2, 0.0)
i += 1
cv.Set1D(point_counts, successes, board_total)
successes += 1
c = cv.WaitKey(15)
x0 = [0.]
#print silly(x0, goodcorners)
print "initial error", silly(x0, goodcorners)
xopt = fmin(silly, x0, args=(goodcorners, ))
print "xopt", xopt
print "final error", silly(xopt, goodcorners)
d = 1.0 # - sum(xopt)
poly = numpy.poly1d(list(xopt) + [d, 0.])
print "final polynomial"
print poly
for co in goodcorners:
scrib = cv.CreateMat(480, 640, cv.CV_8UC3)
cv.SetZero(scrib)
cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints),
[xf(pt, poly) for pt in co], True)
cv.ShowImage("snap", scrib)
cv.WaitKey()
#sys.exit(0)
'''
for (i, (img, (ok, co))) in enumerate(zip(images, corners)):
#scrib = cv.CreateMat(img.rows, img.cols, cv.CV_8UC3)
print "img debug= ",img
scrib = cv.CreateMat(img.rows, img.cols, cv.CV_8UC3)
cv.CvtColor(img, scrib, cv.CV_GRAY2BGR)
if ok:
cv.DrawChessboardCorners(scrib, (num_x_ints, num_y_ints), co, True)
cv.ShowImage("snap", scrib)
cv.WaitKey()
'''
def gather(imagedir, debayer, im_w, im_h):
c=cv.CaptureFromCAM(0)
cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FRAME_WIDTH,im_w)
cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FRAME_HEIGHT,im_h)
#cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FPS,3.75)
grey=cv.CreateImage((im_w,im_h),8,1)
im_cnt = 0
print 'position chess board then press space to find corners'
print 'when complete press q'
if not os.path.exists(imagedir):
os.mkdir(imagedir)
if not os.path.exists(imagedir):
print('failed to create image dir')
sys.exit()
while True:
f=cv.QueryFrame(c)
if (f == None):
print 'failed to capture'
continue
#print f.width, f.height
if (debayer):
bgr=cv.CreateImage((im_w,im_h),8,3)
cv.CvtColor(f,bgr,cv.CV_BayerGR2BGR)
f = bgr
if (f.channels==3):
cv.CvtColor(f,grey,cv.CV_BGR2GRAY)
elif (f.channels==1):
# convert to 8 bit pixel depth
cv.Convert(f,grey)
else:
print 'unsupported colourspace'
break
key = cv.WaitKey(100)
key = key & 255
if not key in range(128):
# show the image
if (im_w > 640):
tmp=cv.CreateImage((im_w/2,im_h/2),8,f.channels)
cv.Resize(f,tmp)
cv.ShowImage("calibrate", tmp)
else:
cv.ShowImage("calibrate", f)
continue
print 'key=0x%x(\'%c\')' % (key, chr(key))
key = chr(key)
if (key == ' '):
print 'looking for corners...'
found,points=cv.FindChessboardCorners(grey,dims,cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if found == 0:
print 'Failed to find corners. Reposition and try again'
else:
cv.DrawChessboardCorners(f,dims,points,found)
#show the final image
if (im_w > 640):
tmp=cv.CreateImage((im_w/2,im_h/2),8,f.channels)
cv.Resize(f,tmp)
cv.ShowImage("calibrate", tmp)
else:
cv.ShowImage("calibrate", f)
#wait indefinitely
print 'Keep this image ? y/n'
key = chr(cv.WaitKey(0) & 255)
if (key == 'y'):
print 'Keeping image ', im_cnt
cv.SaveImage(imagedir+'/calib%05d.pgm' % (im_cnt), grey )
im_cnt+=1
else:
print 'discarding image'
print 'press any key to find next corners'
elif (key == 'q'):
print 'quit'
break
def main(argv):
try:
bag = rosbag.Bag(argv)
except IOError:
print "Bagfile not found ==> exiting"
return
points = []
cv_image = None
bridge = CvBridge()
cv.NamedWindow("L", 1)
cv.NamedWindow("R", 1)
cv.NamedWindow("3D", 1)
prefix = ["left", "right", "kinect_"]
index = 0
for topic, msg, t in bag.read_messages(['/robot_measurement']):
print topic
#if topic=='/robot_measurement_image_left':
#cv_image_l=bridge.imgmsg_to_cv(msg,'bgr8')
#if topic=='/robot_measurement_image_right':
#cv_image_r=bridge.imgmsg_to_cv(msg,'bgr8')
#if topic=='/robot_measurement_image_kinect_rgb':
#cv_image_3d=bridge.imgmsg_to_cv(msg,'bgr8')
cv_image_l = cv.LoadImageM("/tmp/cal/intrinsic/left_%05d.jpg" % index)
cv_image_r = cv.LoadImageM("/tmp/cal/intrinsic/right_%05d.jpg" % index)
cv_image_3d = cv.LoadImageM("/tmp/cal/intrinsic/kinect_%05d.jpg" %
index)
if topic == '/robot_measurement':
points = {'l': [], 'r': [], '3d': []}
for cam in msg.M_cam:
if cam.camera_id == "cam3d":
i = "3d"
elif cam.camera_id == "left":
i = "l"
elif cam.camera_id == "right":
i = "r"
for point in cam.image_points:
points[i].append((point.x, point.y))
#for point in msg.M_cam[0].image_points:
#points['l'].append((point.x,point.y))
#for point in msg.M_cam[1].image_points:
#points['r'].append((point.x,point.y))
#for point in msg.M_cam[2].image_points:
#points['3d'].append((point.x,point.y))
#found,points=cv.FindChessboardCorners(cv_image,(9,6),cv.CV_CALIB_CB_ADAPTIVE_THRESH)
found = 9 * 6
print found
print points
cv.DrawChessboardCorners(cv_image_l, (9, 6), points['l'], found)
cv.DrawChessboardCorners(cv_image_r, (9, 6), points['r'], found)
cv.DrawChessboardCorners(cv_image_3d, (9, 6), points['3d'], found)
cv.ShowImage("L", cv_image_l)
cv.ShowImage("R", cv_image_r)
cv.ShowImage("3D", cv_image_3d)
cv.WaitKey(0)
index += 1
bag.close()
return 0
def handle_msg(self, msg):
# TODO Various asserts that images have same dimension, same board detected...
(lmsg, rmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
epierror = -1
# Get display-images-to-be and detections of the calibration target
lscrib_mono, lcorners, ldownsampled_corners, lboard, (
x_scale, y_scale) = self.downsample_and_detect(lgray)
rscrib_mono, rcorners, rdownsampled_corners, rboard, _ = self.downsample_and_detect(
rgray)
lscrib = cv.CreateMat(lscrib_mono.rows, lscrib_mono.cols, cv.CV_8UC3)
rscrib = cv.CreateMat(rscrib_mono.rows, rscrib_mono.cols, cv.CV_8UC3)
if self.calibrated:
# Show rectified images
lremap = self.l.remap(lgray)
rremap = self.r.remap(rgray)
lrect = lremap
rrect = rremap
if x_scale != 1.0 or y_scale != 1.0:
lrect = cv.CreateMat(lscrib.rows, lscrib.cols, cv.CV_8UC1)
rrect = cv.CreateMat(rscrib.rows, rscrib.cols, cv.CV_8UC1)
cv.Resize(lremap, lrect)
cv.Resize(rremap, rrect)
cv.CvtColor(lrect, lscrib, cv.CV_GRAY2BGR)
cv.CvtColor(rrect, rscrib, cv.CV_GRAY2BGR)
# Draw rectified corners
if lcorners:
src = self.mk_image_points([(lcorners, lboard)])
lundistorted = list(
cvmat_iterator(self.l.undistort_points(src)))
scrib_src = [(x / x_scale, y / y_scale)
for (x, y) in lundistorted]
cv.DrawChessboardCorners(lscrib,
(lboard.n_cols, lboard.n_rows),
scrib_src, True)
if rcorners:
src = self.mk_image_points([(rcorners, rboard)])
rundistorted = list(
cvmat_iterator(self.r.undistort_points(src)))
scrib_src = [(x / x_scale, y / y_scale)
for (x, y) in rundistorted]
cv.DrawChessboardCorners(rscrib,
(rboard.n_cols, rboard.n_rows),
scrib_src, True)
# Report epipolar error
if lcorners and rcorners:
epierror = self.epipolar_error(lundistorted, rundistorted,
lboard)
else:
cv.CvtColor(lscrib_mono, lscrib, cv.CV_GRAY2BGR)
cv.CvtColor(rscrib_mono, rscrib, cv.CV_GRAY2BGR)
# Draw any detected chessboards onto display (downsampled) images
if lcorners:
src = self.mk_image_points([(ldownsampled_corners, lboard)])
cv.DrawChessboardCorners(lscrib,
(lboard.n_cols, lboard.n_rows),
cvmat_iterator(src), True)
if rcorners:
src = self.mk_image_points([(rdownsampled_corners, rboard)])
cv.DrawChessboardCorners(rscrib,
(rboard.n_cols, rboard.n_rows),
cvmat_iterator(src), True)
# Add sample to database only if it's sufficiently different from any previous sample
if lcorners and rcorners:
params = self.get_parameters(lcorners, lboard,
cv.GetSize(lgray))
if self.is_good_sample(params):
self.db.append((params, lgray, rgray))
self.good_corners.append((lcorners, rcorners, lboard))
print "*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple(
[len(self.db)] + params)
rv = StereoDrawable()
rv.lscrib = lscrib
rv.rscrib = rscrib
rv.params = self.compute_goodenough()
rv.epierror = epierror
return rv
#!/usr/bin/python
import sys
import cv
if __name__ == "__main__":
cv.NamedWindow("win")
filename = sys.argv[1]
im = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_GRAYSCALE)
im3 = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_COLOR)
dim = int(sys.argv[2])
status,corners = cv.FindChessboardCorners( im, (dim,dim) )
print corners
print len(corners)
cv.DrawChessboardCorners( im3, (dim,dim), corners, status)
cv.ShowImage("win", im3);
cv.WaitKey()
