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 doPCA(self, X, noComp, folder):
#1) extract the mean of the images out of each image
X = numpy.asfarray(X)
nr, dim = X.shape
meanX = numpy.empty((1, dim), dtype=float)
meanX[0] = X.mean(axis=0)
for i in range(0, nr):
X[i, :] -= meanX[0, :]
#2) compute the projection matrix: (X * X.T) * vi = li * vi
#compute: ui[:,i] = (X.T * vi)/norm(ui[:,i])
otherS = numpy.dot(X, X.T) #compute: (X * X.T)
li, vi = numpy.linalg.eigh(
otherS) #eigenvalues and eigenvectors of (X * X.T)/N
indxs = numpy.argsort(li)
ui = numpy.dot(X.T,
vi[:,
indxs[-noComp:]]) #the formula for the highdim data
#3) normalize the eigenvectors and sort them
for i in range(0, noComp): #normalize the final eigenvectors
ui[:, i] = numpy.divide(ui[:, i], numpy.linalg.norm(ui[:, i]))
#4) store the eigenvectors and the mean
cvEigen = self.array2cv(ui, False)
cv.Save(
"data_train/" + str(folder) + "PcaEigen" + str(self.sizeImg) +
".dat", cvEigen)
cvMean = self.array2cv(meanX, False)
cv.Save(
"data_train/" + str(folder) + "PcaMean" + str(self.sizeImg) +
".dat", cvMean)
def guardarcolores(pixel_rojo):
k = cv.CreateMat(3, 3, cv.CV_64FC1)
k[0, 0] = pixel_rojo[0]
k[0, 1] = pixel_rojo[1]
k[0, 2] = pixel_rojo[2]
cv.Save("Desktop/OpenCV codigos/Mis Codigos/Casco/colores.xml", k)
def convert_dat(path):
os.chdir(path)
dir_list=os.listdir(".")
dat_list=list()
#get all dat files in directory
for file in dir_list:
if os.path.splitext(file)[1]==".dat":
dat_list.append(file)
#training_set_file_stream = open(training_set_list_path,"w")
#convert dat files ony by one
filerange=len(dat_list)
file_ctr=0
for file in dat_list:
f = open(file,"r")
file_content = f.read().strip()
file_content = file_content.replace('\n', ';')
mat=numpy.matrix(file_content)
for(r,c),value in numpy.ndenumerate(mat):
if mat[r,c]==-1:
mat[r,c]=0
mat=mat.astype(float)
mat/=1000
cv_mat=cv.fromarray(mat)
o_path=path+"/"+os.path.splitext(file)[0]+".xml"
cv.Save(o_path,cv_mat,"depthmap")
o_str= "processed file "+str(file_ctr)+ " of "+ str(filerange)
print o_str
file_ctr+=1
def imagen_homografia(n):
global points, width, height,teclado
urllib.urlretrieve("http://192.168.0.100:8080/photo.jpg", "foto.jpg")
foto=cv.LoadImage('foto.jpg')
im_in= cv.CloneImage(foto)
#CALIBRACION
width, height = cv.GetSize(im_in)
im = cv.CloneImage(foto)
if n == 0 and (teclado ==1 or teclado ==3):
cv.ShowImage('Escoger 4 puntos',im)
cv.SetMouseCallback('Escoger 4 puntos', mouse, im)
cv.WaitKey(0)
guardarpoints(points)
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
cv.Save('homography.cvmat', h**o)
else:
points = cargarpoints()
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
out_big = cv.CloneImage(im_in)
cv.WarpPerspective(im_in, out_big, h**o)
out_small = cv.CloneImage(im)
cv.Resize(out_big, out_small)
return out_small, out_big
def export(self, data, year):
with NamedTemporaryFile() as temp:
cv.Save(temp.name, data.bins, name=year)
output = ""
for line in temp:
output += line
return output
def guardarpoints(points):
global teclado
p = cv.CreateMat(4,2,cv.CV_64FC1)
for i in range(4):
(x,y) = points[i]
p[i,0] = x
p[i,1] = y
cv.Save("puntos.xml", p)
def save(self, cv_image, filepath, filename, camera_info):
corrected_filepath = os.path.expanduser(filepath)
fullname = "%s/%s" % (corrected_filepath, filename)
cv.SaveImage(fullname, cv_image)
print "Saved image to %s" % fullname
if self.save_camera_info:
infofullname = fullname.replace(".png", ".pickle")
dump_info(camera_info, infofullname)
print "Saved camera info to %s" % infofullname
intrinsicsfile = fullname.replace(".png", "_intrinsics.xml")
cv.Save(intrinsicsfile,
self.intrinsic_matrix_from_info(camera_info), "intrinsics",
"Intrinsics for our camera")
print "Saved intrinsics to %s" % intrinsicsfile
distfile = fullname.replace(".png", "_distortion.xml")
cv.Save(distfile, self.dist_coeff_from_info(camera_info),
"dist_coeff", "Distortion coefficients for our camera")
print "Saved distortion to %s" % distfile
def on_key_s(frame):
global store_corners, rowcols, intrinsics, distortion
if len(store_corners) < 1:
print "No calibration yet. hold a chessboard in front of the cam and pres <space>"
return
ipts = mk_image_points(store_corners, rowcols)
opts = mk_object_points(len(store_corners), rowcols, 1)
npts = mk_point_counts(len(store_corners), rowcols)
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
distortion = cv.CreateMat(4, 1, cv.CV_64FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(opts,
ipts,
npts,
cv.GetSize(frame),
intrinsics,
distortion,
cv.CreateMat(len(store_corners), 3, cv.CV_32FC1),
cv.CreateMat(len(store_corners), 3, cv.CV_32FC1),
flags=0) # cv.CV_CALIB_ZERO_TANGENT_DIST)
print[[distortion[i, j] for j in range(0, distortion.cols)]
for i in range(0, distortion.rows)]
print[[intrinsics[i, j] for j in range(0, intrinsics.cols)]
for i in range(0, intrinsics.rows)]
cv.Save('intrinsics.xml', intrinsics)
cv.Save('distortion.xml', distortion)
intrinsics = cv.Load('intrinsics.xml')
distortion = cv.Load('distortion.xml')
store_corners = []
def guardarcolores(pixel_rojo, pixel_verde, pixel_azul):
k = cv.CreateMat(3,3,cv.CV_64FC1)
k[0,0] = pixel_rojo[0]
k[0,1] = pixel_rojo[1]
k[0,2] = pixel_rojo[2]
k[1,0] = pixel_verde[0]
k[1,1] = pixel_verde[1]
k[1,2] = pixel_verde[2]
k[2,0] = pixel_azul[0]
k[2,1] = pixel_azul[1]
k[2,2] = pixel_azul[2]
cv.Save("Desktop/OpenCV codigos/Mis Codigos/Casco/colores.xml", k)
def guardarcolores(pixel_rojo, pixel_verde, pixel_azul):
k = cv.CreateMat(3,3,cv.CV_64FC1)
k[0,0] = pixel_rojo[0]
k[0,1] = pixel_rojo[1]
k[0,2] = pixel_rojo[2]
k[1,0] = pixel_verde[0]
k[1,1] = pixel_verde[1]
k[1,2] = pixel_verde[2]
k[2,0] = pixel_azul[0]
k[2,1] = pixel_azul[1]
k[2,2] = pixel_azul[2]
cv.Save("colores.xml", k)
def projPCA(self, X, showIm, folder, sign):
X = numpy.asfarray(X)
#1) Load the eigenVector and the Mean of the data:
eigen = self.cv2array(
cv.Load("data_train/" + str(folder) + "PcaEigen" +
str(self.sizeImg) + ".dat"), False)
meanX = self.cv2array(
cv.Load("data_train/" + str(folder) + "PcaMean" +
str(self.sizeImg) + ".dat"), False)
#2) Substract the mean of the data
for i in range(0, X.shape[0]):
X[i, :] -= meanX[0, :]
#3) do projection on first "?" components: [N,4900]x[4900,?] => [N,?]
projX = numpy.dot(X, eigen)
#5) do back-projection on first "?" components to check: [N,?]x[?,4900] => [N,4900]
if (showIm == True):
backX = numpy.dot(projX, eigen.T)
for i in range(0, X.shape[0]):
backX[i, :] += meanX[0, :] #add the mean back
#6) normalize the backprojection
mini = numpy.min(backX)
backX -= mini
maxi = numpy.max(backX)
backX = numpy.multiply(backX, float(255.0) / float(abs(maxi)))
#7) Shot the backprojected image
eigenHand = self.array2cv(backX[0:1, :], True)
cv.ShowImage("PCA_" + str(sign),
cv.Reshape(eigenHand, 0, self.sizeImg))
print "press any key.."
cv.WaitKey()
#8) store the projection of the data
if (len(sign) > 1):
cvData = self.array2cv(projX, False)
cv.Save(
"data_train/" + str(folder) + str(sign) + "Train" +
str(self.sizeImg) + ".dat", cvData)
return projX
def run_aff_pca(self, features):
'''call aff_face_pca binary for fast PCA computation in C'''
# we need to add an extra id in the start since
#aff_pca_expects it that way
ones = np.zeros((features.shape[0], 1))
temp_feats = np.concatenate((ones, features), axis=1).\
transpose().copy()
temp_dir = mkdtemp()
feature_file = os.path.join(temp_dir, 'features.xml')
cv_mat = cv.fromarray(temp_feats)
cv.Save(feature_file, cv_mat)
pca_file = os.path.join(temp_dir, 'learned_pca.xml')
bin_path = config.bin_dir()
cmd = [
os.path.join(bin_path, 'aff_face_pca'), feature_file, pca_file,
'-n',
str(self.ndims)
]
run_cmd(cmd)
return pca_file
def makeMatrix(self, fold):
files = os.listdir('train/' + fold)
anIndex = 0
matches = glob.glob(os.path.join("train/" + fold, '*.jpg'))
print(self.sizeImg * self.sizeImg)
print len(matches)
imgMatrix = cv.CreateMat(len(matches), (self.sizeImg * self.sizeImg),
cv.CV_8UC1)
oneLine = cv.CreateMat(1, (self.sizeImg * self.sizeImg), cv.CV_8UC1)
resizeImg = cv.CreateMat(self.sizeImg, self.sizeImg, cv.CV_8UC1)
matches.sort()
for aImage in matches:
hands = cv.LoadImageM(aImage, cv.CV_LOAD_IMAGE_GRAYSCALE)
cv.Resize(hands, resizeImg, interpolation=cv.CV_INTER_AREA)
handsMat = cv.Reshape(resizeImg, 0, 1)
for j in range(0, handsMat.width):
imgMatrix[anIndex, j] = handsMat[0, j]
anIndex += 1
cv.Save("data_train/" + fold + "Train" + str(self.sizeImg) + ".dat",
imgMatrix)
file_number = 0
last_file_number = 0
while True:
file_number = file_number + 1
image_name = "%s/%04d.png" % (directory,file_number)
depth_name = "%s/%04d.yml" % (directory,file_number)
if os.path.isfile(image_name) and os.path.isfile(depth_name):
last_file_number = file_number
image = cv.LoadImage(image_name)
depth = cv.Load(depth_name)
cv.ShowImage('image to crop',image)
mh = mouse_handler(image,'image to crop')
while not mh.is_box_drawn():
key = cv.WaitKey(0)
min_x = min(mh.first_corner[0],mh.second_corner[0])
min_y = min(mh.first_corner[1],mh.second_corner[1])
max_x = max(mh.first_corner[0],mh.second_corner[0])
max_y = max(mh.first_corner[1],mh.second_corner[1])
cropped_image = cv.CreateMat(max_y-min_y+1,max_x-min_x+1,cv.CV_8UC3)
cropped_depth = cv.CreateMat(max_y-min_y+1,max_x-min_x+1,cv.CV_32FC3)
for i in range(min_x,max_x+1):
for j in range(min_y,max_y+1):
cropped_image[j-min_y,i-min_x] = image[j,i]
cropped_depth[j-min_y,i-min_x] = depth[j,i]
cv.SaveImage(image_name,cropped_image)
cv.Save(depth_name,cropped_depth)
print 'Cropped '+image_name+' and '+depth_name+'.'
if file_number-last_file_number>1000:
break
for i in range(successes):
cv.Set2D(point_counts2, i, 0, cv.Get2D(point_counts, i, 0))
cv.Set2D(intrinsic, 0, 0, 1.0)
cv.Set2D(intrinsic, 1, 1, 1.0)
rotation_vectors = cv.CreateMat(successes, 3, cv.CV_32FC1)
cv.SetZero(rotation_vectors)
translation_vectors = cv.CreateMat(successes, 3, cv.CV_32FC1)
cv.SetZero(translation_vectors)
cv.CalibrateCamera2(object_points2, image_points2, point_counts2,
cv.GetSize(image), intrinsic, distortion, rotation_vectors,
translation_vectors, 0)
cv.Save("Intrinsics.xml", intrinsic)
cv.Save("Distortion.xml", distortion)
intrinsic = cv.Load("Intrinsics.xml")
distortion = cv.Load("Distortion.xml")
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsic, distortion, mapx, mapy)
cv.NamedWindow("Undistort")
while (image):
t = cv.CloneImage(image)
cv.ShowImage("Raw Video", image)
cv.Remap(t, image, mapx, mapy)
def image_filter(self, cv_image, info, copy=None):
image = cv_image
#Only works on a grayscale image
gray_image = cv.CreateImage(cv.GetSize(image), 8, 1)
cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY)
print "Called with mode: %s" % self.mode
if self.mode == "default" or self.mode == "save_h":
print "Computing homography matrix from checkerboard"
#Get the width and height of the board
board_w = self.cols
board_h = self.rows
#Area of the board = "board_n"
board_n = board_w * board_h
board_sz = (board_w, board_h)
#This needs to be fed with a "height", so it knows how high up the perspective transform should be.
#I've found for the wide_stereo cameras, a value of -15 works well. For the prosilica, -40. Don't ask me why
init_height = self.height
#Uses openCV to find the checkerboard
(found, corners) = cv.FindChessboardCorners(
image, board_sz,
(cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS))
if (not found):
print "Couldn't aquire checkerboard, only found 0 of %d corners\n" % board_n
gr = CloneImage(image)
cv.CvtColor(gray_image, gr, cv.CV_GRAY2BGR)
return gr
#We need subpixel accuracy, so we tell it where the corners are and it magically does the rest. I forget what (11,11) and (-1,-1) mean.
cv.FindCornerSubPix(
gray_image, corners, (11, 11), (-1, -1),
(cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1))
#Pull out the Image Points (3d location of the checkerboard corners, in the camera frame)
#and the Object Points (2d location of the corners on the checkerboard object)
objPts = point_array(4)
imgPts = point_array(4)
objPts[0] = (0, 0)
objPts[1] = (board_w - 1, 0)
objPts[2] = (0, board_h - 1)
objPts[3] = (board_w - 1, board_h - 1)
imgPts[0] = corners[0]
imgPts[1] = corners[board_w - 1]
imgPts[2] = corners[(board_h - 1) * board_w]
imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1]
#Use GetPerspectiveTransform to populate our Homography matrix
H = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(objPts, imgPts, H)
#Since we don't get any z information from this, we populate H[2,2] with our hard-coded height
H[2, 2] = init_height
if self.mode == "save_h":
print "Saving Homography matrix to %s" % self.matrix_location
cv.Save(self.matrix_location, H)
else:
print "Loading Homography matrix from %s" % self.matrix_location
H = cv.Load(self.matrix_location)
birds_image = CloneImage(image)
#birds_image = cv.CreateImage((image.width*3,image.height*3),8,3)
#Uses the homography matrix to warp the perspective.
cv.WarpPerspective(
image, birds_image, H, cv.CV_INTER_LINEAR +
cv.CV_WARP_INVERSE_MAP + cv.CV_WARP_FILL_OUTLIERS)
#Note: If you need to undo the transformation, you can simply invert H and call cv.WarpPerspective again.
return birds_image
cv.NamedWindow("img", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("gray", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("bin_img", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("cnt", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("countours", cv.CV_WINDOW_AUTOSIZE)
cv.CvtColor(img, gray, cv.CV_RGB2GRAY)
cv.InRangeS(gray, cv.Scalar(1), cv.Scalar(250), bin_img)
cv.Canny(bin_img, cnt, 10, 100, 3)
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(bin_img, storage, cv.CV_RETR_TREE,
cv.CV_CHAIN_APPROX_SIMPLE, (0, 0))
while contours != None:
cv.DrawContours(dst, contours, cv.CV_RGB(250, 0, 0), cv.CV_RGB(0, 0, 250),
2, 1, 8)
contours = contours.h_next()
mat = cv.GetMat(dst, 0)
cv.Save('matrix.xml', mat)
cv.ShowImage("img", img)
cv.ShowImage("gray", gray)
cv.ShowImage("bin_img", bin_img)
cv.ShowImage("cnt", cnt)
cv.ShowImage("contours", dst)
cv.WaitKey(0)
def calibrate(gridFiles, gridSize, gridBlockSize):
cpts = []
imageSize = None
for gf in gridFiles:
image = cv.LoadImage(gf, False)
success, corners = cv.FindChessboardCorners(image, gridSize)
corners = cv.FindCornerSubPix(
image, corners, (5, 5), (-1, -1),
(cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1))
gridN = gridSize[0] * gridSize[1]
if len(corners) != gridN:
logging.debug("File failed: %s" % gf)
continue
# fix corners so that the first point is top left
# compare corner[0] to corner[-1]
if corners[0][0] > corners[1][0]:
# flip left/right
logging.debug("Grid is horizontally flipped")
flipped = []
for x in xrange(gridSize[0]):
for y in xrange(gridSize[1]):
cx = gridSize[0] - x - 1
cy = y
flipped.append(corners[cx + cy * gridSize[0]])
corners = flipped
if corners[0][1] > corners[-1][1]:
# flip top/bottom
logging.debug("Grid is vertically flipped")
flipped = []
for x in xrange(gridSize[0]):
for y in xrange(gridSize[1]):
cx = x
cy = gridSize[1] - y - 1
flipped.append(corners[cx + cy * gridSize[0]])
corners = flipped
cpts.append(corners)
imageSize = cv.GetSize(image)
nGrids = len(cpts)
logging.debug("Found %i grids" % nGrids)
if nGrids < 7:
logging.warning("Few grids found: %i" % nGrids)
if nGrids < 5:
raise ValueError("Too few grids: %i" % nGrids)
camMatrix = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetZero(camMatrix)
camMatrix[0, 0] = 1.
camMatrix[1, 1] = 1.
distCoeffs = cv.CreateMat(5, 1, cv.CV_64FC1)
cv.SetZero(distCoeffs)
gridN = gridSize[0] * gridSize[1]
imPts = cv.CreateMat(nGrids * gridN, 2, cv.CV_64FC1)
objPts = cv.CreateMat(nGrids * gridN, 3, cv.CV_64FC1)
ptCounts = cv.CreateMat(nGrids, 1, cv.CV_32SC1)
# organize self.calibrationImgPts (to imPts) and construct objPts and ptCounts
for (i, c) in enumerate(cpts):
for j in xrange(gridN):
imPts[j + i * gridN, 0] = c[j][0]
imPts[j + i * gridN, 1] = c[j][1]
# TODO should thes be actual points? how do I know what they are?
objPts[j + i * gridN, 0] = j % gridSize[0] * gridBlockSize
objPts[j + i * gridN, 1] = j / gridSize[0] * gridBlockSize
objPts[j + i * gridN, 2] = 0.
ptCounts[i, 0] = len(c)
cv.CalibrateCamera2(objPts, imPts, ptCounts, imageSize, camMatrix,
distCoeffs, cv.CreateMat(nGrids, 3, cv.CV_64FC1),
cv.CreateMat(nGrids, 3, cv.CV_64FC1), 0)
cv.Save("camMatrix.xml", camMatrix)
cv.Save("distCoeffs.xml", distCoeffs)
def main():
import sys
csv = np.genfromtxt("syn_groundtruth.csv", delimiter="\t")
x = 1
y = csv.shape[0]
values = np.zeros((y / 2, 13))
freq = 200
q1 = np.zeros((1, 4))
q2 = np.zeros((1, 4))
ind_off = 0
tmp = np.zeros((y - 1 - x, 3))
tmp2 = np.zeros((y - 1 - x, 3))
for nbre in range(x, (y) - 1):
alpha = 0.01
i = nbre - x
tmp2[y - 2 - i -
x, :] = (1 - alpha) * csv[y - i - 1, 6:9] + alpha * csv[y - i - 2,
6:9]
tmp[nbre -
x, :] = (1 - alpha) * csv[nbre - 1, 6:9] + alpha * csv[nbre, 6:9]
csv[x:y - 1, 6:9] = (tmp2[:, :] + tmp[:, :]) * 0.5
# initialize biases
biasa = np.zeros((1, 3))
biasa[0, 0] = -0.3
biasa[0, 1] = -0.4
biasa[0, 2] = 0.02
bias = np.zeros((1, 3))
bias[0, 0] = 0.03
bias[0, 1] = 0.004
bias[0, 2] = 0.08
# simulate IMU measurements and divide the sampling frequency by 2
for nbre in range(x, (y / 2) - 1):
i = nbre - x
j = nbre
rot = np.zeros((1, 9))
rot[0, :] = csv[2 * j - 1, 13:22]
rot = np.reshape(rot, (3, 3))
#generate accelerations
tmp = (csv[2 * j + 1, 6:9] - 2 * csv[2 * j, 6:9] +
csv[2 * j - 1, 6:9]) * freq * freq
values[i, 0:3] = tmp
g = np.zeros((3, 1))
g[2] = 9.81
g[:, 0] += values[i, 0:3].transpose()
g[:] = np.dot(rot.transpose(), g)
values[i, 0:3] = g.transpose()
quatInit = np.zeros((1, 4))
quatInit[0, :] = csv[2 * j - 1, 9:13]
initRot = q2mat(quatInit)
initRot = inv(initRot)
# estimate angular velocity
quatDiff = np.zeros((1, 4))
quatDiff[0, 0:4] = csv[2 * j + 1, 9:13] - csv[2 * j - 1, 9:13]
tmp2 = q2mat(quatDiff * 2)
tmp3 = np.dot(tmp2, initRot)
tmp3 *= 0.5 * freq
# take care of issues occuring for rotations around 180 deg
for vi in range(0, 3):
if i > 0 and (values[i - 1, 3 + vi] * tmp3[0, 1 + vi]) < -0.01:
values[i, 3 + vi] = -tmp3[0, 1 + vi]
else:
values[i, 3 + vi] = tmp3[0, 1 + vi]
values[i, 6:9] = csv[2 * j, 6:9] - csv[
2, 6:
9] #store groundtruth pose by initalizing the drone position to (0,0,0)
values[i, 9:13] = csv[2 * j, 9:13]
#generate biases
bias += np.random.normal(0.0, 0.00000001, (1, 3))
values[i, 3:6] += bias[0, 0:3]
biasa += np.random.normal(0.0, 0.00000001, (1, 3))
values[i, 0:3] += biasa[0, 0:3]
# add noise
values[:, 0:3] = window_mean(values[:, 0:3], 12)
values[:, 0:3] += np.random.normal(0.0, 0.0035, (values.shape[0], 3))
values[:, 3:6] += np.random.normal(0.0, 0.0035, (values.shape[0], 3))
values = np.float32(values)
print(np.mean(values, 0))
opencvmat = cv.fromarray(values)
cv.Save("data.yml", opencvmat)
# Grountdtruth features ********************************************************************
csvPos = np.genfromtxt("featurePos.csv", delimiter="\t")
values = np.zeros((csvPos.shape[0], 3))
print(csv[2, 6:9])
print(csvPos[0, :])
for i in range(0, csvPos.shape[0] - 1):
values[i, :] = csvPos[i, :] - csv[0, 6:9]
values = np.float32(values)
opencvmat = cv.fromarray(values)
cv.Save("featurePos.yml", opencvmat)
directory = '.'
if __name__ == "__main__":
if len(sys.argv) < 2:
print 'No directory specified.'
else:
directory = sys.argv[1]
print 'Writing images to ' + directory
print "With focus on the image window, press space or enter to capture an image, or 'q' or esc to quit."
file_number = 1
for cloud in pc.cloud_source():
image = cv.CreateMat(cloud.height, cloud.width, cv.CV_8UC3)
for (u, v, x, y, z, b, g, r) in pc.points(cloud):
image[v, u] = (b, g, r)
cv.ShowImage('live kinect', image)
key = cv.WaitKey(100)
if key in (space_key, enter_key1, enter_key2):
(image_name, depth_name,
file_number) = new_file_names(directory, file_number)
depth = cv.CreateMat(cloud.height, cloud.width, cv.CV_32FC3)
for (u, v, x, y, z, b, g, r) in pc.points(cloud):
depth[v, u] = (x, y, z)
cv.SaveImage(image_name, image)
cv.Save(depth_name, depth)
print 'Saved ' + image_name + ' and ' + depth_name + '.'
elif key in (q_key, esc_key):
break
elif key != -1:
print 'Unrecognized key pressed:', key
def doSmallManyGabors(self, data, txtLabels, theSign, isPCA):
signs = {"h": ["hands", "garb"], "c": ["rock", "paper", "scissors"]}
#1) compute a set of different gabor filters
lambdas = 4.0 #between 2 and 256
gammas = 0.7 # between 0.2 and 1
psis = 10 #between 0 and 180
thetas = [
0, (numpy.pi / 4.0), (numpy.pi / 2.0), (numpy.pi * 3.0 / 4.0)
] #between (0 and 180) or (-90 and +90)
if (self.pca.sizeImg == 20):
sigmas = 2.0 #between 3 and 68
sizes = 2.0 #between 1 and 10
else:
sigmas = 3.0 #between 3 and 68
sizes = 2.0 #between 1 and 10
#2) store each image as a line at each begining of the row
convo = numpy.empty((data.shape[0], data.shape[1] * (len(thetas) + 1)),
dtype=float)
for j in range(0, data.shape[0]):
for k in range(0, data.shape[1]):
convo[j, k] = data[j, k]
#3) loop over all kernels and convolve them with the images
for i in range(0, len(thetas)):
#4) convolve the images with the gabor filters
self.gabor.setParameters(lambdas, gammas, psis, thetas[i], sigmas,
sizes)
convolved = self.gabor.convolveImg(self.pca.array2cv(data, True),
False)
#5) concatenate the concolved images with the original image on each line
convNumpy = self.pca.cv2array(convolved, True)
for j in range(0, data.shape[0]):
for k in range(0, data.shape[1]):
convo[j, ((i + 1) * data.shape[1]) + k] = convNumpy[j, k]
#5) do PCA on the concatenated (convolved+original) images
finalConv = None
if (isPCA == True): #with PCA
if (data.shape[0] > 1): #not the test image
self.pca.doPCA(convo, self.noComp, "GaborImg/")
else:
finalConv = self.pca.projPCA(convo, False, "GaborImg/", "")
#6) split the set corresponding to labels and store it
if (data.shape[0] > 1): #not the test image
for aSign in signs[theSign]:
signPart = txtLabels[aSign]
signSet = convo[signPart, :]
if (isPCA == True):
#project only what i need out of convo
finalConv = self.pca.projPCA(signSet, False, "GaborImg/",
"")
cv.Save(
"data_train/GaborImg/" + aSign + "Train" +
str(self.pca.sizeImg) + ".dat",
self.pca.array2cv(finalConv, False))
else:
cv.Save(
"data_train/GaborImg/" + aSign + "Train" +
str(self.pca.sizeImg) + ".dat",
self.pca.array2cv(signSet, False))
else:
finalConv = convo
return finalConv
sector = getsector(ox,oy,dx,dy)
cv.Line(sector_temp[sector],(ox,oy),(dx,dy),(32),1,cv.CV_AA)
# accumulate trips into themap
cv.ConvertScale(temp,temp16,1,0);
cv.Add(themap,temp16,themap);
# accumulate sector-separated trips into sector_maps
for sector in range(8):
cv.ConvertScale(sector_temp[sector],sector_temp2[sector],1,0);
cv.Add(sector_maps[sector],sector_temp2[sector],sector_maps[sector])
sector_temp[sector]=cv.CreateMat(height,width,cv.CV_8UC1)
cv.SetZero(sector_temp[sector])
cv.Save(filename,themap)
for sector in range(8):
cv.Save("cache/n%d_c%d_s%d.xml"%(trip_max,cell_size,sector),sector_maps[sector])
lines = cv.CreateMat(height,width,cv.CV_8U)
cv.SetZero(lines)
for trip in all_trips:
for (orig,dest) in pairwise(trip.locations):
oy=height-int(yscale*(orig.latitude - min_lat))
ox=int(xscale*(orig.longitude - min_lon))
dy=height-int(yscale*(dest.latitude - min_lat))
dx=int(xscale*(dest.longitude - min_lon))
cv.Line(lines,(ox,oy),(dx,dy),(255),1,cv.CV_AA)
cv.SaveImage("lines.png",lines)
# cv.MoveWindow ('Coin 3', 375, 325)
# loading the stereo pair
#left = cv.LoadImage('scene_l1.jpg',cv.CV_LOAD_IMAGE_GRAYSCALE)
#right = cv.LoadImage('scene_r1.jpg',cv.CV_LOAD_IMAGE_GRAYSCALE)
print "processing disparity"
disparity_left = cv.CreateMat(left.height, left.width, cv.CV_16S)
disparity_right = cv.CreateMat(left.height, left.width, cv.CV_16S)
#cv.WaitKey()
# data structure initialization
state = cv.CreateStereoGCState(16,2)
# running the graph-cut algorithm
cv.FindStereoCorrespondenceGC(left,right,
disparity_left,disparity_right,state)
disp_left_visual = cv.CreateMat(left.height, left.width, cv.CV_8U)
cv.ConvertScale( disparity_left, disp_left_visual, -16 );
cv.Save( "disparity.pgm", disp_left_visual ); # save the map
# cutting the object farthest of a threshold (120)
cut(disp_left_visual,left,80)
temp2 = cv.GetImage(disp_left_visual)
temp2 = resize_img(temp2, 3)
cv.NamedWindow('Disparity map', cv.CV_WINDOW_AUTOSIZE)
cv.ShowImage('Disparity map', temp2)
cv.WaitKey(150)
time.sleep(1)
cv.Set2D(intrinsic_matrix, 0, 0, 1.0)
cv.Set2D(intrinsic_matrix, 1, 1, 1.0)
rcv = cv.CreateMat(n_boards, 3, cv.CV_64FC1)
tcv = cv.CreateMat(n_boards, 3, cv.CV_64FC1)
print "checking camera calibration............."
# camera calibration
cv.CalibrateCamera2(object_points2, image_points2, point_counts2,
cv.GetSize(image), intrinsic_matrix,
distortion_coefficient, rcv, tcv, 0)
print " checking camera calibration.........................OK "
# storing results in xml files
cv.Save("Intrinsics.xml", intrinsic_matrix)
cv.Save("Distortion.xml", distortion_coefficient)
# Loading from xml files
intrinsic = cv.Load("Intrinsics.xml")
distortion = cv.Load("Distortion.xml")
print " loaded all distortion parameters"
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsic, distortion, mapx, mapy)
cv.NamedWindow("Undistort")
print "all mapping completed"
print "Now relax for some time"
time.sleep(8)
print "now get ready, camera is switching on"
captura = cv.CaptureFromCAM(1)
vid = cv.QueryFrame(captura)
im_in = cv.CloneImage(vid)
#im_in = cv.LoadImage('foto5.jpg', 4)
width, height = cv.GetSize(im_in)
im = cv.CreateImage((640, 480), cv.IPL_DEPTH_8U, 3)
cv.Resize(im_in, im)
cv.ShowImage('Escoger 4 puntos', im)
cv.SetMouseCallback('Escoger 4 puntos', mouse, im)
cv.WaitKey(0)
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
cv.Save('homography.cvmat', h**o)
out = cv.CloneImage(im_in)
cv.WarpPerspective(im_in, out, h**o)
out_small = cv.CloneImage(im)
cv.Resize(out, out_small)
cv.ShowImage('Homografia', out_small)
cv.WaitKey(0)
#captura=cv.CaptureFromCAM(1)
out = cv.CreateImage((640, 480), cv.IPL_DEPTH_8U, 3)
imagen = cv.QueryFrame(captura)
creavideo = cv.CreateVideoWriter("reencode.avi",
cv.CV_FOURCC('M', 'J', 'P', 'G'), 29.97,
(640, 480))
def save_depth_image(image, filename):
cv.Save(filename, image, name="depth")
print "-------------------------------------------------"
print "\n"
cv2.imshow("Test Frame", image)
cv2.waitKey(33)
cv.DestroyWindow("Test Frame")
# camera calibration
h, w = image.shape[:2]
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
[object_points], [image_points], (w, h))
print " checking camera calibration.........................OK "
embed()
# storing results in xml files
cv.Save("Intrinsics.xml", cameraMatrix)
cv.Save("Distortion.xml", distCoeffs)
# Loading from xml files
intrinsic = cv.Load("Intrinsics.xml")
distortion = cv.Load("Distortion.xml")
print " loaded all distortion parameters"
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsic, distortion, mapx, mapy)
cv.NamedWindow("Undistort")
print "all mapping completed"
print "Now relax for some time"
time.sleep(8)
print "now get ready, camera is switching on"
def doManyGabors(self, data, txtLabels, theSign, isPCA):
if (isPCA != True):
self.noComp = data.shape[1]
signs = {"h": ["hands", "garb"], "c": ["rock", "paper", "scissors"]}
#1) compute a set of different gabor filters
lambdas = 4.0 #between 2 and 256
gammas = 0.7 # between 0.2 and 1
psis = 20 #between 0 and 180
thetas = [
0, (numpy.pi / 4.0), (numpy.pi / 2.0), (numpy.pi * 3.0 / 4.0)
] #between (0 and 180) or (-90 and +90)
if (self.pca.sizeImg == 20):
sigmas = 2.0 #between 3 and 68
sizes = 1.0 #between 1 and 10
else:
sigmas = 3.0 #between 3 and 68
sizes = 2.0 #between 1 and 10
#2) loop over all gabor kernels
convo = numpy.empty((data.shape[0], self.noComp * len(thetas)),
dtype=float)
for i in range(0, len(thetas)):
#3) convolve the images with the gabor filters
self.gabor.setParameters(lambdas, gammas, psis, thetas[i], sigmas,
sizes)
convolved = self.gabor.convolveImg(self.pca.array2cv(data, True),
False)
#4) concatenate the concolved images with the original stuff on each line
preConv = self.pca.cv2array(convolved, True)
for j in range(0, data.shape[0]):
for k in range(0, self.noComp):
convo[j, (i * self.noComp) + k] = preConv[j, k]
#5) do PCA on the concatenated convolved images
finalConv = None
if (isPCA == True): #not the test image
print "does PCA"
if (data.shape[0] > 1):
self.pca.doPCA(convo, self.noComp, "Gabor/")
else:
finalConv = self.pca.projPCA(convo, False, "Gabor/", "")
#6) split the set corresponding to labels and store it
if (data.shape[0] > 1): #not the test image
for aSign in signs[theSign]:
signPart = txtLabels[aSign]
signSet = convo[signPart, :]
if (isPCA == True):
finalConv = self.pca.projPCA(signSet, False, "Gabor/", "")
cv.Save(
"data_train/Gabor/" + aSign + "Train" +
str(self.pca.sizeImg) + ".dat",
self.pca.array2cv(finalConv, False))
else:
cv.Save(
"data_train/Gabor/" + aSign + "Train" +
str(self.pca.sizeImg) + ".dat",
self.pca.array2cv(signSet, False))
else:
finalConv = convo
return finalConv
