def solve(C, Q, matches, dbsiftpath, dbimgpath):
# open EarthMine info
info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
em = render_tags.EarthmineImageInfo(dbimgpath, info)
map3d = C.pixelmap.open(dbsiftpath)
# find non-None features
vector = []
for i, m in enumerate(matches):
d = m['db']
# get 3d pt of feature
feature = map3d[int(d[0]), int(d[1])]
if not feature:
continue
# convert from latlon to meters relative to earthmine camera
pz, px = geom.lltom(em.lat, em.lon, feature['lat'], feature['lon'])
py = feature['alt'] - em.alt
vector.append((m['query'][:2], (px, py, -pz)))
print vector[0]
# reference camera matrix
# f 0 0
# 0 f 0
# 0 0 1
A = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetZero(A)
f = 662 # focal len?
cv.Set2D(A, 0, 0, cv.Scalar(f))
cv.Set2D(A, 1, 1, cv.Scalar(f))
cv.Set2D(A, 2, 2, cv.Scalar(1))
# convert vector to to cvMats
objectPoints3d = cv.CreateMat(len(vector), 1, cv.CV_64FC3)
imagePoints2d = cv.CreateMat(len(vector), 1, cv.CV_64FC2)
for i, (p2d, p3d) in enumerate(vector):
cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*p2d))
cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*p3d))
coeff = cv.CreateMat(4, 1, cv.CV_64F)
rvec = cv.CreateMat(3, 1, cv.CV_64F)
tvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(coeff)
cv.SetZero(rvec)
cv.SetZero(tvec)
# since rvec, tvec are zero the initial guess is the earthmine camera
ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
coeff, rvec, tvec, useExtrinsicGuess=False)
np_rvec = np.matrix(rvec)
np_tvec = np.matrix(tvec)
print np_rvec
print np_tvec
return np_rvec, np_tvec
def do_calibration(self):
if not self.goodenough:
print "Can not calibrate yet!"
return
#append all things in db
good_corners = [
corners for (params, corners, object_points) in self.db
]
good_points = [
object_points for (params, corners, object_points) in self.db
]
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
distortion = cv.CreateMat(8, 1, cv.CV_64FC1) # rational polynomial
else:
distortion = cv.CreateMat(5, 1, cv.CV_64FC1) # plumb bob
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
opts = self.mk_object_points(good_points)
ipts = self.mk_image_points(good_corners)
npts = self.mk_point_counts(good_points)
cv.CalibrateCamera2(opts,
ipts,
npts,
self.size,
intrinsics,
distortion,
cv.CreateMat(len(good_corners), 3, cv.CV_32FC1),
cv.CreateMat(len(good_corners), 3, cv.CV_32FC1),
flags=self.calib_flags)
self.intrinsics = intrinsics
self.distortion = distortion
# R is identity matrix for monocular calibration
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.R)
self.P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(self.P)
ncm = cv.GetSubRect(self.P, (0, 0, 3, 3))
cv.GetOptimalNewCameraMatrix(self.intrinsics, self.distortion,
self.size, self.alpha, ncm)
print self.size
self.calibrated = True
return (self.distortion, self.intrinsics, self.R, self.P)
def compute_flow_opencv(alpha, iterations, ifile1, ifile2):
import cv
i1 = cv.LoadImageM(os.path.join("flow", ifile1), iscolor=False)
i2 = cv.LoadImageM(os.path.join("flow", ifile2), iscolor=False)
u = cv.CreateMat(i1.rows, i1.cols, cv.CV_32F)
cv.SetZero(u)
v = cv.CreateMat(i1.rows, i1.cols, cv.CV_32F)
cv.SetZero(v)
l = 1.0/(alpha**2)
cv.CalcOpticalFlowHS(i1, i2, 0, u, v, l, (cv.CV_TERMCRIT_ITER, iterations, 0))
# return blitz arrays
return numpy.array(u, 'float64'), numpy.array(v, 'float64')
def calibrate(self):
image_points_mat = concatenate_mats(self.image_points)
object_points_mat = concatenate_mats(self.object_points)
print "Image Points:"
for row in range(image_points_mat.height):
for col in range(image_points_mat.width):
print image_points_mat[row, col]
print
print "Object Points:"
for row in range(object_points_mat.height):
for col in range(object_points_mat.width):
print object_points_mat[row, col]
print
point_counts_mat = cv.CreateMat(1, self.number_of_scenes, cv.CV_32SC1)
for i in range(self.number_of_scenes):
point_counts_mat[0, i] = self.image_points[i].width
intrinsics = cv.CreateMat(3, 3, cv.CV_32FC1)
distortion = cv.CreateMat(4, 1, cv.CV_32FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
size = (self.projector_info.width, self.projector_info.height)
tvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
rvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
cv.CalibrateCamera2(object_points_mat,
image_points_mat,
point_counts_mat,
size,
intrinsics,
distortion,
rvecs,
tvecs,
flags=0)
R = cv.CreateMat(3, 3, cv.CV_32FC1)
P = cv.CreateMat(3, 4, cv.CV_32FC1)
cv.SetIdentity(R)
cv.SetZero(P)
cv.Copy(intrinsics, cv.GetSubRect(P, (0, 0, 3, 3)))
self.projector_info = create_msg(size, distortion, intrinsics, R, P)
rospy.loginfo(self.projector_info)
for col in range(3):
for row in range(self.number_of_scenes):
print tvecs[row, col],
print
def projectPixelTo3d(self, left_uv, disparity):
"""
:param left_uv: rectified pixel coordinates
:type left_uv: (u, v)
:param disparity: disparity, in pixels
:type disparity: float
Returns the 3D point (x, y, z) for the given pixel position,
using the cameras' :math:`P` matrices.
This is the inverse of :meth:`project3dToPixel`.
Note that a disparity of zero implies that the 3D point is at infinity.
"""
src = mkmat(4, 1, [left_uv[0], left_uv[1], disparity, 1.0])
dst = cv.CreateMat(4, 1, cv.CV_64FC1)
cv.SetZero(dst)
cv.MatMul(self.Q, src, dst)
x = dst[0, 0]
y = dst[1, 0]
z = dst[2, 0]
w = dst[3, 0]
if w != 0:
return (x / w, y / w, z / w)
else:
return (0.0, 0.0, 0.0)
def compute_glcm(img, d): order = 8 newimg = quantize(img, order) glcm = cv.CreateMat(order,order,cv.CV_8UC1) normglcm = cv.CreateMat(order, order, cv.CV_32FC1) cv.SetZero(glcm) div = 255/order for i in range(img.rows-d): for j in range(img.cols-d): val1 = cv.Get2D(newimg, i, j) val2 = cv.Get2D(newimg, i+d, j+d) p = int(val1[0]/div) q = int(val2[0]/div) if p>=order: p = order -1 if q>=order: q = order -1 #print p, q val3 = cv.Get2D(glcm, p, q) cv.Set2D(glcm, p, q, (val3[0]+1)) tot = cv.Sum(glcm) for i in range(glcm.rows): for j in range(glcm.cols): val3 = cv.Get2D(glcm, i , j) val = 1.0*val3[0]/tot[0] cv.Set2D(normglcm, i, j, (val)) #print round(float(cv.Get2D(normglcm, i, j)[0]), 3), #print "\n" return normglcm
def draw(self, mousePos=None):
'''
Computes the image montage from the source images based on the current
image pointer (position in list of images), etc. This internally constructs
the montage, but show() is required for display and mouse-click handling.
'''
cv.SetZero(self._cvMontageImage)
img_ptr = self._imgPtr
if img_ptr > 0:
#we are not showing the first few images in imageList
#so display the decrement arrow
cv.FillConvexPoly(self._cvMontageImage, self._decrArrow, (125,125,125))
if img_ptr + (self._rows*self._cols) < len(self._images):
#we are not showing the last images in imageList
#so display increment arrow
cv.FillConvexPoly(self._cvMontageImage, self._incrArrow, (125,125,125))
if self._byrow:
for row in range(self._rows):
for col in range(self._cols):
if img_ptr > len(self._images)-1: break
tile = pv.Image(self._images[img_ptr].asAnnotated())
self._composite(tile, (row,col), img_ptr)
img_ptr += 1
else:
for col in range(self._cols):
for row in range(self._rows):
if img_ptr > len(self._images)-1: break
tile = pv.Image(self._images[img_ptr].asAnnotated())
self._composite(tile, (row,col), img_ptr)
img_ptr += 1
def loop(self):
frame = []
trash = []
while self.active:
self.loops += 1
batch = []
#f not len(OCVThread1.BUFFER) > 1: time.sleep(0.01)
lock.acquire()
i = 0
while OCVThread1.BUFFER and i < 30:
batch.append(OCVThread1.BUFFER.pop())
i += 1
lock.release()
if not batch:
time.sleep(0.01)
trash = []
while batch:
P = batch.pop()
if P.index == 0:
self.update_frame(frame) # final draws and shows
cv.SetZero(self.contours_image)
frame = [Shape(P.poly2, P.depth)]
else:
frame.append(Shape(P.poly2, P.depth))
P.draw(self.contours_image) # do this outside of update_frame
s = frame[-1]
s.draw_defects(self.contours_image)
s.draw_bounds(self.contours_image)
s.draw_variance(self.contours_image)
#del P # can randomly segfault?, let python GC del P
#trash.append( P )
print('iter thread2', self.loops)
print('ocv thread2 exit', self.loops)
def draw_contour(self, contour, size):
# create an empty one-channel image
contour_im = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
cv.SetZero(contour_im) # make sure nothing is in the image
cv.DrawContours(contour_im, contour, cv.Scalar(255), cv.Scalar(255), 0,
cv.CV_FILLED)
return contour_im
def getWatershedMask(self):
'''
Uses the watershed algorithm to refine the foreground mask.
Currently, this doesn't work well on real video...maybe grabcut would be better.
'''
cvMarkerImg = cv.CreateImage(self._fgMask.size, cv.IPL_DEPTH_32S, 1)
cv.SetZero(cvMarkerImg)
#fill each contour with a different gray level to label connected components
seq = self._contours
c = 50
while not (seq == None) and len(seq) != 0:
if cv.ContourArea(seq) > self._minArea:
c += 10
moments = cv.Moments(seq)
m00 = cv.GetSpatialMoment(moments, 0, 0)
m01 = cv.GetSpatialMoment(moments, 0, 1)
m10 = cv.GetSpatialMoment(moments, 1, 0)
centroid = (int(m10 / m00), int(m01 / m00))
cv.Circle(cvMarkerImg, centroid, 3, cv.RGB(c, c, c),
cv.CV_FILLED)
seq = seq.h_next()
if (c > 0):
img = self._annotateImg.asOpenCV()
cv.Watershed(img, cvMarkerImg)
tmp = cv.CreateImage(cv.GetSize(cvMarkerImg), cv.IPL_DEPTH_8U, 1)
cv.CvtScale(cvMarkerImg, tmp)
return pv.Image(tmp)
def on_trackbar(position):
# create the image for putting in it the founded contours
contours_image = cv.CreateImage((_SIZE, _SIZE), 8, 3)
# compute the real level of display, given the current position
levels = position - 3
# initialisation
_contours = contours
if levels <= 0:
# zero or negative value
# => get to the nearest face to make it look more funny
_contours = contours.h_next().h_next().h_next()
# first, clear the image where we will draw contours
cv.SetZero(contours_image)
# draw contours in red and green
cv.DrawContours(contours_image, _contours, _red, _green, levels, 3,
cv.CV_AA, (0, 0))
# finally, show the image
cv.ShowImage("contours", contours_image)
def getForegroundPixels(self, bgcolor=None):
'''
@param bgcolor: The background color to use. Specify as an (R,G,B) tuple.
Specify None for a blank/black background.
@return: The full color foreground pixels on either a blank (black)
background, or on a background color specified by the user.
@note: You must call detect() before getForegroundPixels() to
get updated information.
'''
if self._fgMask == None: return None
#binary mask selecting foreground regions
mask = self._fgMask.asOpenCVBW()
#full color source image
image = self._annotateImg.copy().asOpenCV()
#dest image, full color, but initially all zeros (black/background)
# we will copy the foreground areas from image to here.
dest = cv.CloneImage(image)
if bgcolor == None:
cv.SetZero(dest)
else:
cv.Set(dest, cv.RGB(*bgcolor))
cv.Copy(image, dest,
mask) #copy only pixels from image where mask != 0
return pv.Image(dest)
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 __init__(self):
super(Video, self).__init__()
self.LoadIniData('tracker.ini')
self.storage = cv.CreateMemStorage(0)
self.last = 0
self.old_center = (0, 0)
self.capture = cv.CaptureFromCAM(self.cameraid)
if not self.capture:
print "Error opening capture device"
sys.exit(1)
self.frame = cv.QueryFrame(self.capture)
self.size = cv.GetSize(self.frame)
(self.width, self.height) = self.size
self.motion = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
self.eye = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
self.mhi = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.orient = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.segmask = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.mask = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 1)
self.temp = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 3)
self.buf = range(10)
for i in range(self.n_frames):
self.buf[i] = cv.CreateImage(self.size, cv.IPL_DEPTH_8U, 1)
cv.SetZero(self.buf[i])
def on_trackbar(position):
# create the image for putting in it the founded contours
contours_image = cv.CreateImage((_SIZEW, _SIZEH), 8, 3)
scale_factor = max(1.0 * _SIZEW / _MSIZEW, 1.0 * _SIZEH / _MSIZEH)
contours_image_to_display = cv.CreateImage(
(_SIZEW / scale_factor, _SIZEH / scale_factor), 8, 3)
# compute the real level of display, given the current position
levels = position - 3
# initialisation
_contours = contours
#if levels <= 0:
# # zero or negative value
# # => get to the nearest face to make it look more funny
# _contours = [] #contours.h_next().h_next().h_next()
# first, clear the image where we will draw contours
cv.SetZero(contours_image)
# draw contours in red and green
cv.DrawContours(contours_image, _contours, _red, _green, levels, 3,
cv.CV_AA, (0, 0))
# finally, show the image
cv.Resize(contours_image, contours_image_to_display, cv.CV_INTER_CUBIC)
cv.ShowImage("contours", contours_image_to_display)
def hsv_filter(src,
low_h,
high_h,
min_s,
max_s,
min_v,
max_v,
hue_bandstop=False):
'''Takes an 8-bit bgr src image and does simple hsv thresholding.
A binary image of the same size will be returned. HSV values have the
following ranges:
Hue - 0 to 360
Saturation - 0 to 255
Value - 0 to 255
If hue_bandstop is True, the low_h and high_h will act as a band stop
filter on hue, otherwise hue will be a band pass filter.
'''
# OpenCV expects hue to be ranged from 0-180, since 0-360 wouldn't fit
# inside a byte.
high_h /= 2
low_h /= 2
hsv = cv.CreateImage(cv.GetSize(src), 8, 3)
binary = cv.CreateImage(cv.GetSize(src), 8, 1)
cv.SetZero(binary)
cv.CvtColor(src, hsv, cv.CV_BGR2HSV)
data = hsv.tostring()
for y in xrange(0, hsv.height):
for x in xrange(0, hsv.width):
index = y * hsv.width * 3 + x * 3
h = ord(data[0 + index])
s = ord(data[1 + index])
v = ord(data[2 + index])
if hue_bandstop:
if (h <= low_h or h >= high_h) and \
s >= min_s and \
s <= max_s and \
v >= min_v and \
v <= max_v:
cv.Set2D(binary, y, x, (255, ))
else:
if h >= low_h and \
h <= high_h and \
s >= min_s and \
s <= max_s and \
v >= min_v and \
v <= max_v:
cv.Set2D(binary, y, x, (255, ))
return binary
def rotate(src, center, angle):
mapMatrix = cv.CreateMat(2,3,cv.CV_64F)
#cv.GetRotationMatrix2D( center, 450 - angle, 1.0, mapMatrix)
cv.GetRotationMatrix2D( center, 270 - angle, 1.0, mapMatrix)
dst = cv.CreateImage( cv.GetSize(src), src.depth, src.nChannels)
cv.SetZero(dst)
cv.WarpAffine(src, dst, mapMatrix)
return dst
def filter_color(image, color):
mask = color_mask(image, color)
if mask:
new_img = cv.CreateImage((image.width, image.height), image.depth,
image.nChannels)
cv.SetZero(new_img)
cv.Copy(image, new_img, mask)
return new_img
def cal_fromcorners(self, good):
"""
:param good: Good corner positions and boards
:type good: [(corners, ChessboardInfo)]
"""
boards = [b for (_, b) in good]
ipts = self.mk_image_points(good)
opts = self.mk_object_points(boards)
npts = self.mk_point_counts(boards)
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
distortion = cv.CreateMat(8, 1, cv.CV_64FC1) # rational polynomial
else:
distortion = cv.CreateMat(5, 1, cv.CV_64FC1) # plumb bob
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(opts,
ipts,
npts,
self.size,
intrinsics,
distortion,
cv.CreateMat(len(good), 3, cv.CV_32FC1),
cv.CreateMat(len(good), 3, cv.CV_32FC1),
flags=self.calib_flags)
self.intrinsics = intrinsics
self.distortion = distortion
# R is identity matrix for monocular calibration
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.R)
self.P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(self.P)
self.mapx = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.mapy = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.set_alpha(0.0)
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 stitch_stacking(iplimg1, iplimg2):
assert iplimg1.depth == iplimg2.depth
assert iplimg2.channels == iplimg2.channels
total_width = max(iplimg1.width, iplimg2.width)
total_height = iplimg1.height + iplimg2.height
size = (total_width, total_height)
iplimage = cv.CreateImage(size, iplimg1.depth, iplimg1.channels)
cv.SetZero(iplimage)
paste(iplimg1, iplimage, 0, 0)
paste(iplimg2, iplimage, 0, iplimg1.height)
return iplimage
def draw(self, im, threshold):
new_im = image.new_from(im)
edges = image.edges(im, threshold, threshold * 3, 3)
cv.SetZero(new_im)
cv.Copy(im, new_im, edges)
size = cv.GetSize(im)
print cv.CountNonZero(image.threshold(edges)) / float(
size[0] * size[1])
#cv.Dilate(new_im, new_im)
#cv.Erode(new_im, new_im)
return new_im
def addGaussianNoise(image_path, save_path):
img = cv.LoadImage(image_path)
noise = cv.CreateImage(cv.GetSize(img), img.depth, img.nChannels)
cv.SetZero(noise)
rng = cv.RNG(-1)
cv.RandArr(rng, noise, cv.CV_RAND_NORMAL, cv.ScalarAll(0),
cv.ScalarAll(25))
cv.Add(img, noise, img)
tempName = os.path.splitext(
os.path.basename(image_path))[0] + "_noised.jpg"
save_image = os.path.join(save_path, tempName)
cv.SaveImage(save_image, img)
def get_circular_roi(self, center, rad, src):
# create an empty one-channel image
outside = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_8U, 1)
cv.SetZero(outside) # make sure nothing is in the image
# create white (filled) circle, by passing -1
cv.Circle(outside, center, rad, ColorSpace.RGB_WHITE, -1)
# invert to create the circular window
cv.Not(outside, outside)
# Subtract the background from image to get only the are desired ROI
roi = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_8U, 1)
cv.Sub(src, outside, roi)
return roi
def __init__(self):
grid_spacing = rospy.get_param('~grid_spacing')
self.bridge = CvBridge()
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('projector/get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'projector/get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('projector/set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('projector/set_projection',
projector.srv.SetProjection)
projector_info = projector_info_service().projector_info
projector_model = image_geometry.PinholeCameraModel()
projector_model.fromCameraInfo(projector_info)
# Generate grid projections
self.original_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
for row in range(0, projector_info.height, grid_spacing):
cv.Line(self.original_projection, (0, row),
(projector_info.width - 1, row), 255)
for col in range(0, projector_info.width, grid_spacing):
cv.Line(self.original_projection, (col, 0),
(col, projector_info.height - 1), 255)
predistortmap_x = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
InitPredistortMap(projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs(), predistortmap_x,
predistortmap_y)
self.predistorted_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
cv.Remap(self.original_projection,
self.predistorted_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=(0, 0, 0, 0))
self.off_projection = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_8UC1)
cv.SetZero(self.off_projection)
def process_motion(self,img):
center = (-1, -1)
# a lot of stuff from this section was taken from the code motempl.py,
# openCV's python sample code
timestamp = time.clock() / self.clocks_per_sec # get current time in seconds
idx1 = self.last
cv.CvtColor(img, self.buf[self.last], cv.CV_BGR2GRAY) # convert frame to grayscale
idx2 = (self.last + 1) % self.n_frames
self.last = idx2
silh = self.buf[idx2]
cv.AbsDiff(self.buf[idx1], self.buf[idx2], silh) # get difference between frames
cv.Threshold(silh, silh, 30, 1, cv.CV_THRESH_BINARY) # and threshold it
cv.UpdateMotionHistory(silh, self.mhi, timestamp, self.mhi_duration) # update MHI
cv.ConvertScale(self.mhi, self.mask, 255./self.mhi_duration,
(self.mhi_duration - timestamp)*255./self.mhi_duration)
cv.SetZero(img)
cv.Merge(self.mask, None, None, None, img)
cv.CalcMotionGradient(self.mhi, self.mask, self.orient, self.max_time_delta, self.min_time_delta, 3)
seq = cv.SegmentMotion(self.mhi, self.segmask, self.storage, timestamp, self.max_time_delta)
inc = 0
a_max = 0
max_rect = -1
# there are lots of things moving around
# in this case just find find the biggest change on the image
for (area, value, comp_rect) in seq:
if comp_rect[2] + comp_rect[3] > 60: # reject small changes
if area > a_max:
a_max = area
max_rect = inc
inc += 1
# found it, now just do some processing on the area.
if max_rect != -1:
(area, value, comp_rect) = seq[max_rect]
color = cv.CV_RGB(255, 0,0)
silh_roi = cv.GetSubRect(silh, comp_rect)
# calculate number of points within silhouette ROI
count = cv.Norm(silh_roi, None, cv.CV_L1, None)
# this rectangle contains the overall motion ROI
cv.Rectangle(self.motion, (comp_rect[0], comp_rect[1]),
(comp_rect[0] + comp_rect[2],
comp_rect[1] + comp_rect[3]), (0,0,255), 1)
# the goal is to report back a center of movement contained in a rectangle
# adjust the height based on the number generated by the slider bar
h = int(comp_rect[1] + (comp_rect[3] * (float(self.height_value) / 100)))
# then calculate the center
center = ((comp_rect[0] + comp_rect[2] / 2), h)
return center
def __init__(self,
sample_image,
max_area,
adaptation_rate=0.8,
threshold=10):
self.max_area = max_area
self.accumulator = cv.CreateImage(cv.GetSize(sample_image), 32, 1)
cv.SetZero(self.accumulator)
self.thresholded_img = cv.CreateImage(cv.GetSize(sample_image), 8, 1)
self.difference_img = cv.CreateImage(cv.GetSize(sample_image), 32, 1)
self.green32_img = cv.CreateImage(cv.GetSize(sample_image), 32, 1)
self.adaptation_rate = adaptation_rate
self.threshold = threshold
def extrinsic(frame):
frame.lower()
rvec = cv.CreateMat(1,3,cv.CV_32FC1)
cv.SetZero(rvec)
rvec_0 = rospy.get_param('/camera_' + frame + '_rvec_0')
rvec_1 = rospy.get_param('/camera_' + frame + '_rvec_1')
rvec_2 = rospy.get_param('/camera_' + frame + '_rvec_2')
cv.SetReal2D(rvec,0,0,rvec_0)
cv.SetReal2D(rvec,0,1,rvec_1)
cv.SetReal2D(rvec,0,2,rvec_2)
tvec = cv.CreateMat(1,3,cv.CV_32FC1)
cv.SetZero(tvec)
tvec_0 = rospy.get_param('/camera_' + frame + '_tvec_0')
tvec_1 = rospy.get_param('/camera_' + frame + '_tvec_1')
tvec_2 = rospy.get_param('/camera_' + frame + '_tvec_2')
cv.SetReal2D(tvec,0,0,tvec_0)
cv.SetReal2D(tvec,0,1,tvec_1)
cv.SetReal2D(tvec,0,2,tvec_2)
return rvec, tvec
def __init__(self):
self.active = False
self.depth16raw = OCVThread1.DEPTH16RAW
self.depth16 = cv.CreateImage((640, 480), cv.IPL_DEPTH_16S, 1)
self.depth8 = cv.CreateImage((640, 480), 8, 1)
self.sweep_thresh = [
cv.CreateImage((640, 480), 8, 1) for i in range(SWEEPS)
]
self.storage = cv.CreateMemStorage(0)
#self.storage_poly = cv.CreateMemStorage(0)
gui.NamedWindow('depth', 1)
cv.SetZero(self.depth16raw)
def __init__(self):
self.printed_chessboard_corners_x = rospy.get_param(
'~printed_chessboard_corners_x')
self.printed_chessboard_corners_y = rospy.get_param(
'~printed_chessboard_corners_y')
self.printed_chessboard_spacing = rospy.get_param(
'~printed_chessboard_spacing')
self.bridge = CvBridge()
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
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.")
projector_info_service = rospy.ServiceProxy(
'get_projector_info', projector.srv.GetProjectorInfo)
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)
rospy.loginfo("Waiting for projector info setting service...")
rospy.wait_for_service('set_projector_info')
rospy.loginfo("Projection setting service found.")
self.set_projector_info = rospy.ServiceProxy(
'set_projector_info', sensor_msgs.srv.SetCameraInfo)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.projector_info = projector_info_service().projector_info
self.blank_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.SetZero(self.blank_projection)
self.number_of_scenes = 0
self.object_points = []
self.image_points = []
