def normalize_plane(plane, aggressive=0):
if aggressive:
# smooth = image_empty_clone(plane)
# cv.Smooth(plane, smooth, cv.CV_GAUSSIAN, 13, 13)
hist = get_gray_histogram(plane, bins=255)
_, max_value, _, max_color = cv.GetMinMaxHistValue(hist)
thr_value = max_value * aggressive
down_threshold, up_threshold = None, None
for k in range(256):
down_val = cv.QueryHistValue_1D(hist, k)
up_val = cv.QueryHistValue_1D(hist, 254 - k)
if down_threshold is None and down_val >= thr_value:
down_threshold = k
if up_threshold is None and up_val >= thr_value:
up_threshold = k
if down_threshold is not None and up_threshold is not None:
break
sub_plane = None
if down_threshold > 0:
sub_plane = image_empty_clone(plane)
cv.SubS(plane, down_threshold, sub_plane)
add_plane = None
if down_threshold + up_threshold > 0:
add_plane = image_empty_clone(plane)
cv.AddS(sub_plane or plane, down_threshold + up_threshold,
add_plane)
plane = add_plane or plane
norm_plane = image_empty_clone(plane)
cv.Normalize(plane, norm_plane, 0, 255, cv.CV_MINMAX)
return norm_plane
def createModelsfromStats(self):
cv.ConvertScale(self.IavgF, self.IavgF, float(1.0 / self.Icount))
cv.ConvertScale(self.IdiffF, self.IdiffF, float(1.0 / self.Icount))
cv.AddS(self.IdiffF, cv.Scalar(1.0, 1.0, 1.0), self.IdiffF)
self.setHighThresh(200.0)
self.setLowThresh(200.0)
def setUp(self):
size = 100
offset = 20
so = size - offset
angle = 45
move = (0, 0)
scale = 0.5
center = (size / 2, size / 2)
time = 1
points = [(offset, offset), (so, offset), (so, so), (offset, so)]
img = cv.CreateImage((size, size), 8, 1)
cv.SetZero(img)
points = rotate_points(points, center, angle)
corners = [Corner(points, 0), Corner(points, 1), Corner(points, 2), Corner(points, 3)]
npoints = rotate_points(points, center, 3)
npoints = map(lambda x:add((2, 2), int_point(x)), npoints)
npoints = standarize_contours(npoints)
ncorners = [Corner(npoints, 0,imgtime=1), Corner(npoints, 1,imgtime=1), Corner(npoints, 2,imgtime=1), Corner(npoints, 3,imgtime=1)]
for i, cor in enumerate(ncorners):
cor.compute_change(corners[i])
self.points = npoints
self.old_corners = corners
self.corners = ncorners
self.size = size
cv.AddS(img, 255, img)
self.draw_img = img
self.timg = cv.CloneImage(img)
self.tmp_img = cv.CloneImage(img)
pass
def createModelsfromStats():
cv.ConvertScale(IavgF, IavgF, float(1.0 / Icount))
cv.ConvertScale(IdiffF, IdiffF, float(1.0 / Icount))
cv.AddS(IdiffF, cv.Scalar(1.0, 1.0, 1.0), IdiffF)
setHighThresh(10.0)
setLowThresh(10.0)
def __add__(self, addend):
"""Add two images together -> cvImg
"""
bmp = self.empty()
if isinstance(addend, Img):
cv.Add(self.cv_rep(), addend.cv_rep(), bmp)
else:
cv.AddS(self.cv_rep(), cv.Scalar(addend, addend, addend), bmp)
return bmp
def FormatImage(img, oimg, off, scale, correction):
global i01
#print img.height,img.width
#print oimg.height,oimg.width
cv.Transpose(img,oimg)
cv.Flip(oimg,None,0)
if(correction):
cv.AddS(oimg, off, oimg)
cv.Div(oimg, i01, oimg, scale)
def display_masks(image, mask_array, cv_bridge):
for mask in mask_array.masks:
img = cv.GetImage(image)
pt1 = (mask.roi.x, mask.roi.y)
pt2 = (mask.roi.x + mask.roi.width, mask.roi.y + mask.roi.height)
cv.Rectangle(image, pt1, pt2, cv.Scalar(255, 0, 0), 2)
if len(mask.mask.data) != 0:
cv.SetImageROI(
img, (mask.roi.x, mask.roi.y, mask.roi.width, mask.roi.height))
cv_mask = cv_bridge.imgmsg_to_cv(mask.mask)
cv.AddS(img, (0, 50.0, 0), img, cv_mask)
cv.ResetImageROI(img)
def crunch(): size = cv.GetSize(band3) assert size == cv.GetSize(band4) numerator = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Sub(band4, band3, numerator) denominator = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Add(band4, band3, denominator) ndvi_img = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Div(numerator, denominator, ndvi_img) # (NDVI + 1) cv.AddS(ndvi_img, 1, ndvi_img) return ndvi_img
def test2():
path = "/Users/soswow/Documents/Face Detection/gray_test"
for file_path, name in directory_files(path):
if name.endswith(".jpg"):
img = cv.LoadImage(file_path, iscolor=False)
N = sizeOf(img)[0] * sizeOf(img)[1]
I_bar = cv.Sum(img)[0] / N
tmp = image_empty_clone(img)
cv.SubS(img, I_bar, tmp)
tmp2 = image_empty_clone(img)
cv.AddS(tmp, 128, tmp2)
merged = merge_images(img, tmp2, vertical=True)
cv.SaveImage(join(path, "_%s" % name), merged)
def _queryFrame(self):
# frame = cv.QueryFrame(self._cameraDevice)
# if self.mirrored:
# mirroredFrame = cv.CreateImage(cv.GetSize(frame), 12, \
# frame.nChannels)
# cv.Flip(frame, mirroredFrame, 1)
# frame = mirroredFrame
#
# if self.brightness != 0:
# cv.AddS(frame, cv.Scalar(self.brightness,self.brightness,self.brightness), frame);
#
# if self.contrast != 1:
# cv.Scale(frame,frame, self.contrast);
#
# self.newFrame.emit(frame)
source = cv2.imread(loadImage2P.getLatestImage())
frame = cv.CreateImageHeader((source.shape[1], source.shape[0]), cv.IPL_DEPTH_8U, 3)
cv.SetData(frame, source.tostring(), source.dtype.itemsize * 3 * source.shape[1])
# Now let opencv decompress your image
#frame = cv.DecodeImage(pi, cv.CV_LOAD_IMAGE_COLOR)
#frame = cv.GetImage(preframe, cv.IPL_DEPTH_8U, 3)
if self.mirrored:
mirroredFrame = cv.CreateImage(cv.GetSize(frame), 12, \
frame.nChannels)
cv.Flip(frame, mirroredFrame, 1)
frame = mirroredFrame
if self.brightness != 0:
cv.AddS(frame, cv.Scalar(self.brightness,self.brightness,self.brightness), frame);
if self.contrast != 1:
cv.Scale(frame,frame, self.contrast);
(w, h) = cv.GetSize(frame)
if (w,h) != self.imageSize:
self.imageSize = (w,h)
self.parent.setGeometry(3,20,w,h)
self.parent.display.setFixedSize(w,h)
self.parent.getGrid().setScreenSize(w,h)
self.newFrame.emit(frame)
def glyphRec(image):
storage = cv.CreateMemStorage(0)
contrast = cv.CreateImage(cv.GetSize(image), 8, 3)
grey = cv.CreateImage(cv.GetSize(image), 8, 1)
canny = cv.CreateImage(cv.GetSize(grey), cv.IPL_DEPTH_8U, 1)
#increase contrast
avg = cv.Avg(image)
cv.AddS(image, cv.Scalar(-.5 * avg[0], -.5 * avg[1], -.5 * avg[2]),
contrast)
cv.Scale(contrast, contrast, 3)
#make grayscale
cv.CvtColor(contrast, grey, cv.CV_BGR2GRAY)
#smooth
cv.Smooth(grey, grey, cv.CV_GAUSSIAN, 3, 3)
#edge detect
cv.Canny(grey, canny, 20, 200, 3)
#smooth again
cv.Smooth(canny, canny, cv.CV_GAUSSIAN, 3, 3)
#find lines
lines = cv.HoughLines2(canny, storage, cv.CV_HOUGH_PROBABILISTIC, 3,
math.pi / 180, 50, 150, 40)
#find corners
corners = getCorners(lines)
#find quadrilaterals
quad = findGlpyh(contrast, corners)
if quad == None:
return None
drawQuad(image, quad)
grid = readGlyph(image, quad)
printGrid(grid)
print ''
toCoords(grid)
return grid
def extract_hue_channel(self, cv_hue_image, bound_range):
size = (cv_hue_image.width, cv_hue_image.height)
tmp = cv.CreateImage(size, 8, 1)
mask = cv.CreateImage(size, 8, 1)
start, end = bound_range
cv.Zero(tmp)
cv.InRangeS(cv_hue_image, int(start), int(end), mask)
# copy pixels in range from hue
cv.AddS(cv_hue_image, 1, tmp, mask)
avg_hue = cv.Avg(tmp, mask)
avg_hue = avg_hue[0]
return tmp, avg_hue
def scanline_numbers_to_planes(self, scanline_numbers):
rows = scanline_numbers.height
cols = scanline_numbers.width
normal_vectors_x = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_x, -1)
normal_vectors_y = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_y, 0)
normal_vectors_z = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Copy(scanline_numbers, normal_vectors_z)
cv.ConvertScale(normal_vectors_z,
normal_vectors_z,
scale=self.pixels_per_scanline)
cv.AddS(normal_vectors_z, -self.center_pixel, normal_vectors_z)
cv.ConvertScale(normal_vectors_z,
normal_vectors_z,
scale=1.0 / self.projector_model.fx())
normal_vectors = cv.CreateMat(rows, cols, cv.CV_32FC3)
cv.Merge(normal_vectors_x, normal_vectors_y, normal_vectors_z, None,
normal_vectors)
# Bring the normal vectors into camera coordinates
cv.Transform(normal_vectors, normal_vectors,
self.projector_to_camera_rotation_matrix)
normal_vectors_split = [None] * 3
for i in range(3):
normal_vectors_split[i] = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Split(normal_vectors, normal_vectors_split[0],
normal_vectors_split[1], normal_vectors_split[2], None)
n_dot_p = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(n_dot_p)
for i in range(3):
cv.ScaleAdd(normal_vectors_split[i],
self.projector_to_camera_translation_vector[i],
n_dot_p, n_dot_p)
planes = cv.CreateMat(rows, cols, cv.CV_32FC4)
cv.Merge(normal_vectors_split[0], normal_vectors_split[1],
normal_vectors_split[2], n_dot_p, planes)
return planes
def pre_process_source(self, cv_source_image):
size = (cv_source_image.width, cv_source_image.height)
src = cv.CloneImage(cv_source_image)
hue = cv.CreateImage(size, 8, 1)
mask = cv.CreateImage(size, 8, 1)
tmp = cv.CreateImage(size, 8, 3)
cv.Zero(tmp)
cv.Resize(cv_source_image, src)
cv.Smooth(src, src, cv.CV_GAUSSIAN, 13, 13)
cv.CvtColor(src, src, cv.CV_BGR2HSV)
#rough thresholding to filter out noisie
cv.InRangeS(src, self.config.treshold_hsv, [181, 256, 256], mask)
cv.AddS(src, [1, 1, 1, 1], tmp, mask)
cv.Split(tmp, hue, None, None, None)
return hue
def project_pixels_to_3d_rays(pixels, model):
x = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC1)
y = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC1)
cv.Split(pixels, x, y, None, None)
x_squared = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC1)
cv.Pow(x, x_squared, 2)
y_squared = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC1)
cv.Pow(y, y_squared, 2)
inverse_norm = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC1)
cv.Add(x_squared, y_squared, inverse_norm)
cv.AddS(inverse_norm, 1, inverse_norm)
cv.Pow(inverse_norm, inverse_norm, -0.5)
cv.Mul(x, inverse_norm, x)
cv.Mul(y, inverse_norm, y)
result = cv.CreateMat(pixels.height, pixels.width, cv.CV_32FC3)
cv.Merge(x, y, inverse_norm, None, result)
return result
# use nonzero_rows parameter in cv.FT() call below
cv.DFT(dft_A, dft_A, cv.CV_DXT_FORWARD, complexInput.height)
cv.NamedWindow("win", 0)
cv.NamedWindow("magnitude", 0)
cv.ShowImage("win", im)
# Split Fourier in real and imaginary parts
cv.Split(dft_A, image_Re, image_Im, None, None)
# Compute the magnitude of the spectrum Mag = sqrt(Re^2 + Im^2)
cv.Pow(image_Re, image_Re, 2.0)
cv.Pow(image_Im, image_Im, 2.0)
cv.Add(image_Re, image_Im, image_Re, None)
cv.Pow(image_Re, image_Re, 0.5)
# Compute log(1 + Mag)
cv.AddS(image_Re, cv.ScalarAll(1.0), image_Re, None) # 1 + Mag
cv.Log(image_Re, image_Re) # log(1 + Mag)
# Rearrange the quadrants of Fourier image so that the origin is at
# the image center
cvShiftDFT(image_Re, image_Re)
min, max, pt1, pt2 = cv.MinMaxLoc(image_Re)
cv.Scale(image_Re, image_Re, 1.0 / (max - min), 1.0 * (-min) / (max - min))
cv.ShowImage("magnitude", image_Re)
cv.WaitKey(0)
def atualiza_foto(self):
real = cv.CreateImage(cv.GetSize(imagem), cv.IPL_DEPTH_64F, 1)
imaginario = cv.CreateImage(cv.GetSize(imagem), cv.IPL_DEPTH_64F, 1)
complexo = cv.CreateImage(cv.GetSize(imagem), cv.IPL_DEPTH_64F, 2)
cv.Scale(imagem_cinza, real, 1.0, 0.0)
cv.Zero(imaginario)
cv.Merge(real, imaginario, None, None, complexo)
Altura_M = cv.GetOptimalDFTSize(imagem.height - 1)
Largura_N = cv.GetOptimalDFTSize(imagem.width - 1)
Vetor_dft = cv.CreateMat(Altura_M, Largura_N, cv.CV_64FC2)
imagem_Real = cv.CreateImage((Largura_N, Altura_M), cv.IPL_DEPTH_64F,
1)
imagem_Imaginaria = cv.CreateImage((Largura_N, Altura_M),
cv.IPL_DEPTH_64F, 1)
temporario = cv.GetSubRect(Vetor_dft,
(0, 0, imagem.width, imagem.height))
cv.Copy(complexo, temporario, None)
if (Vetor_dft.width > imagem.width):
temporario = cv.GetSubRect(
Vetor_dft,
(imagem.width, 0, Largura_N - imagem.width, imagem.height))
cv.Zero(temporario)
# APLICANDO FOURIER
cv.DFT(Vetor_dft, Vetor_dft, cv.CV_DXT_FORWARD, complexo.height)
cv.Split(Vetor_dft, imagem_Real, imagem_Imaginaria, None, None)
cv.Pow(imagem_Real, imagem_Real, 2.0)
cv.Pow(imagem_Imaginaria, imagem_Imaginaria, 2.0)
cv.Add(imagem_Real, imagem_Imaginaria, imagem_Real, None)
cv.Pow(imagem_Real, imagem_Real, 0.5)
cv.AddS(imagem_Real, cv.ScalarAll(1.0), imagem_Real, None)
cv.Log(imagem_Real, imagem_Real)
cvShiftDFT(imagem_Real, imagem_Real)
min, max, pt1, pt2 = cv.MinMaxLoc(imagem_Real)
cv.Scale(imagem_Real, imagem_Real, 1.0 / (max - min),
1.0 * (-min) / (max - min))
#APLICANDO FILTRO passa-baixa circular
cv.Circle(Vetor_dft, (0, 0), self.raio, [0, 0, 0], -1, 1, 0)
cv.Circle(Vetor_dft, (Vetor_dft.cols, 0), self.raio, [0, 0, 0], -1, 1,
0)
cv.Circle(Vetor_dft, (0, Vetor_dft.rows), self.raio, [0, 0, 0], -1, 1,
0)
cv.Circle(Vetor_dft, (Vetor_dft.cols, Vetor_dft.rows), self.raio,
[0, 0, 0], -1, 1, 0)
cv.Split(Vetor_dft, imagem_Real, imagem_Imaginaria, None, None)
cv.Pow(imagem_Real, imagem_Real, 2.0)
cv.Pow(imagem_Imaginaria, imagem_Imaginaria, 2.0)
cv.Add(imagem_Real, imagem_Imaginaria, imagem_Real, None)
cv.Pow(imagem_Real, imagem_Real, 0.5)
cv.AddS(imagem_Real, cv.ScalarAll(1.0), imagem_Real, None)
cv.Log(imagem_Real, imagem_Real)
cvShiftDFT(imagem_Real, imagem_Real)
min, max, pt1, pt2 = cv.MinMaxLoc(imagem_Real)
cv.Scale(imagem_Real, imagem_Real, 1.0 / (max - min),
1.0 * (-min) / (max - min))
cv.ShowImage("Transformada de Fourier", imagem_Real)
# APLICANDO A INVERSA de Fourier
cv.DFT(Vetor_dft, Vetor_dft, cv.CV_DXT_INVERSE_SCALE, Largura_N)
cv.Split(Vetor_dft, imagem_Real, imagem_Imaginaria, None, None)
min, max, pt1, pt2 = cv.MinMaxLoc(imagem_Real)
if ((pt1 < 0) or (pt2 > 255)):
cv.Scale(imagem_Real, imagem_Real, 1.0 / (max - min),
1.0 * (-min) / (max - min))
else:
cv.Scale(imagem_Real, imagem_Real, 1.0 / 255, 0)
cv.ShowImage("Inversa da Fourier", imagem_Real)
def __SSIM(self, frame1, frame2):
"""
The equivalent of Zhou Wang's SSIM matlab code using OpenCV.
from http://www.cns.nyu.edu/~zwang/files/research/ssim/index.html
The measure is described in :
"Image quality assessment: From error measurement to structural similarity"
C++ code by Rabah Mehdi. http://mehdi.rabah.free.fr/SSIM
C++ to Python translation and adaptation by Iñaki Úcar
"""
C1 = 6.5025
C2 = 58.5225
img1_temp = self.__array2cv(frame1)
img2_temp = self.__array2cv(frame2)
nChan = img1_temp.nChannels
d = cv.IPL_DEPTH_32F
size = img1_temp.width, img1_temp.height
img1 = cv.CreateImage(size, d, nChan)
img2 = cv.CreateImage(size, d, nChan)
cv.Convert(img1_temp, img1)
cv.Convert(img2_temp, img2)
img1_sq = cv.CreateImage(size, d, nChan)
img2_sq = cv.CreateImage(size, d, nChan)
img1_img2 = cv.CreateImage(size, d, nChan)
cv.Pow(img1, img1_sq, 2)
cv.Pow(img2, img2_sq, 2)
cv.Mul(img1, img2, img1_img2, 1)
mu1 = cv.CreateImage(size, d, nChan)
mu2 = cv.CreateImage(size, d, nChan)
mu1_sq = cv.CreateImage(size, d, nChan)
mu2_sq = cv.CreateImage(size, d, nChan)
mu1_mu2 = cv.CreateImage(size, d, nChan)
sigma1_sq = cv.CreateImage(size, d, nChan)
sigma2_sq = cv.CreateImage(size, d, nChan)
sigma12 = cv.CreateImage(size, d, nChan)
temp1 = cv.CreateImage(size, d, nChan)
temp2 = cv.CreateImage(size, d, nChan)
temp3 = cv.CreateImage(size, d, nChan)
ssim_map = cv.CreateImage(size, d, nChan)
#/*************************** END INITS **********************************/
#// PRELIMINARY COMPUTING
cv.Smooth(img1, mu1, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.Smooth(img2, mu2, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.Pow(mu1, mu1_sq, 2)
cv.Pow(mu2, mu2_sq, 2)
cv.Mul(mu1, mu2, mu1_mu2, 1)
cv.Smooth(img1_sq, sigma1_sq, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma1_sq, 1, mu1_sq, -1, 0, sigma1_sq)
cv.Smooth(img2_sq, sigma2_sq, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma2_sq, 1, mu2_sq, -1, 0, sigma2_sq)
cv.Smooth(img1_img2, sigma12, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma12, 1, mu1_mu2, -1, 0, sigma12)
#//////////////////////////////////////////////////////////////////////////
#// FORMULA
#// (2*mu1_mu2 + C1)
cv.Scale(mu1_mu2, temp1, 2)
cv.AddS(temp1, C1, temp1)
#// (2*sigma12 + C2)
cv.Scale(sigma12, temp2, 2)
cv.AddS(temp2, C2, temp2)
#// ((2*mu1_mu2 + C1).*(2*sigma12 + C2))
cv.Mul(temp1, temp2, temp3, 1)
#// (mu1_sq + mu2_sq + C1)
cv.Add(mu1_sq, mu2_sq, temp1)
cv.AddS(temp1, C1, temp1)
#// (sigma1_sq + sigma2_sq + C2)
cv.Add(sigma1_sq, sigma2_sq, temp2)
cv.AddS(temp2, C2, temp2)
#// ((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
cv.Mul(temp1, temp2, temp1, 1)
#// ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
cv.Div(temp3, temp1, ssim_map, 1)
index_scalar = cv.Avg(ssim_map)
#// through observation, there is approximately
#// 1% error max with the original matlab program
return index_scalar[0]
def get_point_cloud(self):
# Scan the scene
pixel_associations = self.get_pixel_associations()
# Project white light onto the scene to get the intensities of each picture (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
projector_model = PinholeCameraModel()
projector_model.fromCameraInfo(self.projector_info)
projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector,
projector_to_camera_rotation_matrix)
pixel_associations_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
pixel_associations_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Split(pixel_associations, pixel_associations_x,
pixel_associations_y, None, None)
valid_points_mask_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_x, -1, valid_points_mask_x, cv.CV_CMP_NE)
valid_points_mask_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_y, -1, valid_points_mask_y, cv.CV_CMP_NE)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.And(valid_points_mask_x, valid_points_mask_y, valid_points_mask)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
rectified_projector_coordinates = cv.CreateMat(1,
number_of_valid_points,
cv.CV_32FC2)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
rectified_projector_coordinates[0, i] = pixel_associations[
row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to pixel numbers
cv.AddS(rectified_projector_coordinates, 0.5,
rectified_projector_coordinates)
cv.ConvertScale(rectified_projector_coordinates,
rectified_projector_coordinates,
self.pixels_per_scanline)
# Rectify the projector pixels
cv.UndistortPoints(rectified_projector_coordinates,
rectified_projector_coordinates,
projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs())
camera_rays = projectPixelsTo3dRays(rectified_camera_coordinates,
camera_model)
projector_rays = projectPixelsTo3dRays(rectified_projector_coordinates,
projector_model)
# Bring the projector rays into camera coordinates
cv.Transform(projector_rays, projector_rays,
projector_to_camera_rotation_matrix)
camera_centers = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC3)
cv.SetZero(camera_centers)
projector_centers = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC3)
cv.Set(projector_centers, self.projector_to_camera_translation_vector)
intersection_points = line_line_intersections(camera_centers,
camera_rays,
projector_centers,
projector_rays)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def __init__(self):
self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
self.scanner_info_file_name = rospy.get_param(
'~scanner_info_file_name')
self.threshold = rospy.get_param('~threshold')
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
self.bridge = CvBridge()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info',
sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
get_projector_info = rospy.ServiceProxy('get_projector_info',
projector.srv.GetProjectorInfo)
self.projector_info = get_projector_info().projector_info
self.projector_model = PinholeCameraModel()
self.projector_model.fromCameraInfo(self.projector_info)
self.read_scanner_info()
self.projector_to_camera_rotation_matrix = cv.CreateMat(
3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_matrix)
predistortmap_x = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_32FC1)
InitPredistortMap(self.projector_model.intrinsicMatrix(),
self.projector_model.distortionCoeffs(),
predistortmap_x, predistortmap_y)
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
cv.AddS(predistortmap_x, -minVal, predistortmap_x)
uncropped_projection_width = int(math.ceil(maxVal - minVal))
self.center_pixel = self.projector_model.cx() - minVal
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
cv.AddS(predistortmap_y, -minVal, predistortmap_y)
uncropped_projection_height = int(math.ceil(maxVal - minVal))
self.number_of_scanlines = int(
math.ceil(
float(uncropped_projection_width) / self.pixels_per_scanline))
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(self.number_of_scanlines):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = int(
math.ceil(math.log(self.number_of_scanlines, 2)))
self.predistorted_positive_projections = []
self.predistorted_negative_projections = []
for i in range(self.number_of_projection_patterns):
cross_section = cv.CreateMat(1, uncropped_projection_width,
cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(uncropped_projection_width):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(graycodes[scanline],
i) * 255
cross_section[0, pixel] = scanline_value
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(uncropped_projection_height,
uncropped_projection_width,
cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
predistorted_positive_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width,
cv.CV_8UC1)
cv.Remap(positive_projection,
predistorted_positive_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN)
#Create a negative of the pattern for thresholding
predistorted_negative_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width,
cv.CV_8UC1)
cv.Not(predistorted_positive_projection,
predistorted_negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
predistorted_positive_projection_faded = fade_edges(
predistorted_positive_projection, 20)
predistorted_negative_projection_faded = fade_edges(
predistorted_negative_projection, 20)
self.predistorted_positive_projections.append(
predistorted_positive_projection_faded)
self.predistorted_negative_projections.append(
predistorted_negative_projection_faded)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection',
projector.srv.SetProjection)
self.pixel_associations_msg = None
self.point_cloud_msg = None
rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud,
self.handle_point_cloud_srv)
point_cloud_pub = rospy.Publisher('~point_cloud',
sensor_msgs.msg.PointCloud)
rospy.loginfo("Ready.")
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()
def parsekey(k):
global step
global scale
global ofs
global off
global correction
global lines
global text
global gp
lowbyte = k & 0xff
specialbit = (k & 0x8000)>0
if(lowbyte == ord('a') and specialbit == False):
step = step * 2
print "step:", step
elif(lowbyte == ord('z') and specialbit == False):
step = step / 2
print "step:", step
elif(lowbyte == 81 and specialbit == True): #left
MoveMillRel(step,0,0)
elif(lowbyte == 83 and specialbit == True): #right
MoveMillRel(-step,0,0)
elif(lowbyte == 82 and specialbit == True): #up
MoveMillRel(0,-step,0)
elif(lowbyte == 84 and specialbit == True): #dn
MoveMillRel(0,step,0)
elif(lowbyte == 85 and specialbit == True): #pgup
MoveMillRel(0,0,-step)
elif(lowbyte == 86 and specialbit == True): #pgdn
MoveMillRel(0,0,step)
elif(lowbyte == 201 and specialbit == True): #f12
cmd = "set spindle increase\r\n"
s.sendall(cmd)
print readline(s)
elif(lowbyte == 200 and specialbit == True): #f11
cmd = "set spindle decrease\r\n"
s.sendall(cmd)
print readline(s)
elif(lowbyte == ord('=') and specialbit == False):
scale = scale + 5
print "scale:", scale
elif(lowbyte == ord('-') and specialbit == False):
scale = scale - 5
print "scale:", scale
elif(lowbyte == ord('+') and specialbit == False):
ofs = ofs + 5
print "ofs:",ofs
off = (-pmin+ofs,-pmin+ofs,-pmin+ofs)
cv.AddS(i01, (5, 5, 5), i01)
elif(lowbyte == ord('_') and specialbit == False):
ofs = ofs - 5
print "ofs:",ofs
off = (-pmin+ofs,-pmin+ofs,-pmin+ofs)
cv.AddS(i01, (-5, -5, -5), i01)
elif(lowbyte == ord('c') and specialbit == False):
correction = not correction
print "correction:",correction
elif(lowbyte == ord('l') and specialbit == False):
lines = not lines
print "lines:",lines
elif(lowbyte == ord('t') and specialbit == False):
text = not text
print "text:",text
elif(lowbyte == ord('w') and specialbit == False):
avgnimages(il,5,0)
name = ("%02.3f_%02.3f_%02.3f.jpg") % (gp.x,gp.y,gp.z)
cv.SaveImage(name,list(il)[0])
print "wrote ", name
################################################################
#get calibration data
i01 = cv.LoadImage('white-calib/calib.png')
passvals = cv.Load('white-calib/passvals.xml')
imin = cv.GetReal1D(passvals,0)
imax = cv.GetReal1D(passvals,1)
pmin = cv.GetReal1D(passvals,2)
#scale = pmin
scale = 100
ofs = 0
off = (-pmin+ofs,-pmin+ofs,-pmin+ofs)
#off = (-imin, -imin, -imin)
#off = (-pmin, -pmin, -pmin)
cv.AddS(i01, off, i01)
################################################################
# setup camera
cv.NamedWindow("camera", 1)
capture = cv.CreateCameraCapture(0)
width = None #leave None for auto-detection
height = None #leave None for auto-detection
if width is None:
width = int(cv.GetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH))
else:
cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FRAME_WIDTH,width)
if height is None:
def doSSIM(frame1, frame2):
'''
The equivalent of Zhou Wang's SSIM matlab code using OpenCV.
from http://www.cns.nyu.edu/~zwang/files/research/ssim/index.html
The measure is described in :
"Image quality assessment: From error measurement to structural similarity"
C++ code by Rabah Mehdi. http://mehdi.rabah.free.fr/SSIM
C++ to Python translation and adaptation by Iñaki Úcar
'''
def array2cv(a):
dtype2depth = {
'uint8': cv.IPL_DEPTH_8U,
'int8': cv.IPL_DEPTH_8S,
'uint16': cv.IPL_DEPTH_16U,
'int16': cv.IPL_DEPTH_16S,
'int32': cv.IPL_DEPTH_32S,
'float32': cv.IPL_DEPTH_32F,
'float64': cv.IPL_DEPTH_64F,
}
try:
nChannels = a.shape[2]
except:
nChannels = 1
cv_im = cv.CreateImageHeader((a.shape[1], a.shape[0]),
dtype2depth[str(a.dtype)], nChannels)
cv.SetData(cv_im, a.tostring(),
a.dtype.itemsize * nChannels * a.shape[1])
return cv_im
C1 = 6.5025
C2 = 58.5225
img1_temp = array2cv(frame1)
img2_temp = array2cv(frame2)
nChan = img1_temp.nChannels
d = cv.IPL_DEPTH_32F
size = img1_temp.width, img1_temp.height
img1 = cv.CreateImage(size, d, nChan)
img2 = cv.CreateImage(size, d, nChan)
cv.Convert(img1_temp, img1)
cv.Convert(img2_temp, img2)
img1_sq = cv.CreateImage(size, d, nChan)
img2_sq = cv.CreateImage(size, d, nChan)
img1_img2 = cv.CreateImage(size, d, nChan)
cv.Pow(img1, img1_sq, 2)
cv.Pow(img2, img2_sq, 2)
cv.Mul(img1, img2, img1_img2, 1)
mu1 = cv.CreateImage(size, d, nChan)
mu2 = cv.CreateImage(size, d, nChan)
mu1_sq = cv.CreateImage(size, d, nChan)
mu2_sq = cv.CreateImage(size, d, nChan)
mu1_mu2 = cv.CreateImage(size, d, nChan)
sigma1_sq = cv.CreateImage(size, d, nChan)
sigma2_sq = cv.CreateImage(size, d, nChan)
sigma12 = cv.CreateImage(size, d, nChan)
temp1 = cv.CreateImage(size, d, nChan)
temp2 = cv.CreateImage(size, d, nChan)
temp3 = cv.CreateImage(size, d, nChan)
ssim_map = cv.CreateImage(size, d, nChan)
#/*************************** END INITS **********************************/
#// PRELIMINARY COMPUTING
cv.Smooth(img1, mu1, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.Smooth(img2, mu2, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.Pow(mu1, mu1_sq, 2)
cv.Pow(mu2, mu2_sq, 2)
cv.Mul(mu1, mu2, mu1_mu2, 1)
cv.Smooth(img1_sq, sigma1_sq, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma1_sq, 1, mu1_sq, -1, 0, sigma1_sq)
cv.Smooth(img2_sq, sigma2_sq, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma2_sq, 1, mu2_sq, -1, 0, sigma2_sq)
cv.Smooth(img1_img2, sigma12, cv.CV_GAUSSIAN, 11, 11, 1.5)
cv.AddWeighted(sigma12, 1, mu1_mu2, -1, 0, sigma12)
#//////////////////////////////////////////////////////////////////////////
#// FORMULA
#// (2*mu1_mu2 + C1)
cv.Scale(mu1_mu2, temp1, 2)
cv.AddS(temp1, C1, temp1)
#// (2*sigma12 + C2)
cv.Scale(sigma12, temp2, 2)
cv.AddS(temp2, C2, temp2)
#// ((2*mu1_mu2 + C1).*(2*sigma12 + C2))
cv.Mul(temp1, temp2, temp3, 1)
#// (mu1_sq + mu2_sq + C1)
cv.Add(mu1_sq, mu2_sq, temp1)
cv.AddS(temp1, C1, temp1)
#// (sigma1_sq + sigma2_sq + C2)
cv.Add(sigma1_sq, sigma2_sq, temp2)
cv.AddS(temp2, C2, temp2)
#// ((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
cv.Mul(temp1, temp2, temp1, 1)
#// ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2))
cv.Div(temp3, temp1, ssim_map, 1)
index_scalar = cv.Avg(ssim_map)
#// through observation, there is approximately
#// 1% error max with the original matlab program
return index_scalar[0]
