def stereoRectify1(self, im1, im1_r):
cv.Remap(im1,
im1_r,
self.rect_map1x,
self.rect_map1y,
flags=cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS,
fillval=0)
def remap(img, mapy, mapx, interp=2):
"""
transform image using coordinate x,y
Interpolation method:
0 = CV_INTER_NN nearest-neigbor interpolation
1 = CV_INTER_LINEAR bilinear interpolation (used by default)
2 = CV_INTER_CUBIC bicubic interpolation
3 = CV_INTER_AREA resampling using pixel area relation. It is the preferred method for image decimation that gives moire-free results. In terms of zooming it is similar to the CV_INTER_NN method
return resulting array
"""
des = N.empty_like(img)
# cv.fromarray: array can be 2D or 3D only
if cv2.__version__.startswith('2'):
cimg = cv.fromarray(img)
cdes = cv.fromarray(des)
cmapx = cv.fromarray(mapx.astype(N.float32))
cmapy = cv.fromarray(mapy.astype(N.float32))
cv.Remap(cimg, cdes, cmapx, cmapy, flags=interp+cv.CV_WARP_FILL_OUTLIERS)
else:
cimg = img
cdes = des
cmapx = mapx.astype(N.float32)
cmapy = mapy.astype(N.float32)
cdes = cv2.remap(cimg, cmapx, cmapy, interp)
return N.asarray(cdes)
def dewarp(imagedir):
# Loading from json file
C = CameraParams()
C.load(imagedir+"/params.json")
K = cv.fromarray(C.K)
D = cv.fromarray(C.D)
print "loaded camera parameters"
mapx = None
mapy = None
for f in os.listdir(imagedir):
if (f.find('pgm')<0):
continue
image = imagedir+'/'+f
print image
original = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE)
dewarped = cv.CloneImage(original);
# setup undistort map for first time
if (mapx == None or mapy == None):
im_dims = (original.width, original.height)
mapx = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
mapy = cv.CreateImage( im_dims, cv.IPL_DEPTH_32F, 1 );
cv.InitUndistortMap(K,D,mapx,mapy)
cv.Remap( original, dewarped, mapx, mapy )
tmp1=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(original,tmp1)
tmp2=cv.CreateImage((im_dims[0]/2,im_dims[1]/2),8,1)
cv.Resize(dewarped,tmp2)
cv.ShowImage("Original", tmp1 )
cv.ShowImage("Dewarped", tmp2)
cv.WaitKey(-1)
def ffts(self, im_small):
im_arr = np.asarray(im_small)
im_arr = im_arr - im_arr.mean()
im_arr = im_arr * self.mask_motion
im_fft = np.fft.fftshift(
np.fft.fft2(im_arr,
(self.fft_size, self.fft_size))) # save for matching
self.fftw_in[:] = 0.
self.fftw_in[0:im_small.rows, 0:im_small.cols] += im_arr
self.fftw_plan.execute()
im_fft = np.zeros(self.fftw_out.shape, self.fftw_out.dtype)
im_fft += self.fftw_out
im_fft = np.fft.fftshift(im_fft)
im_fft_mag = np.abs(im_fft * self.high_pass)
# log polar
im_fft_im = cv.fromarray(np.float32(im_fft_mag))
cv.Remap(im_fft_im, self.im_lp, self.logpolar_mapx, self.logpolar_mapy,
cv.CV_INTER_LINEAR)
im_lp = np.asarray(self.im_lp) * self.mask_lp
#cv.SaveImage(filename, cv.fromarray(im_lp))
im_lp = im_lp - np.mean(im_lp)
im_lp_fft = np.fft.fft2(
im_lp, (self.fft_size, self.fft_size)) # save for matching
self.fftw_in[:] = 0.
self.fftw_in += im_lp
self.fftw_plan.execute()
im_lp_fft = np.zeros(self.fftw_out.shape, self.fftw_out.dtype)
im_lp_fft += self.fftw_out
return (im_fft, im_lp_fft)
def main(argv):
if len(argv) < 10:
print('Usage: %s input-file fx fy cx cy k1 k2 p1 p2 output-file' %
argv[0])
sys.exit(-1)
src = argv[1]
fx, fy, cx, cy, k1, k2, p1, p2, output = argv[2:]
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
intrinsics[0, 0] = float(fx)
intrinsics[1, 1] = float(fy)
intrinsics[2, 2] = 1.0
intrinsics[0, 2] = float(cx)
intrinsics[1, 2] = float(cy)
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = float(k1)
dist_coeffs[0, 1] = float(k2)
dist_coeffs[0, 2] = float(p1)
dist_coeffs[0, 3] = float(p2)
src = cv.LoadImage(src)
dst = cv.CreateImage(cv.GetSize(src), src.depth, src.nChannels)
mapx = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, dist_coeffs, mapx, mapy)
cv.Remap(src, dst, mapx, mapy,
cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS, cv.ScalarAll(0))
# cv.Undistort2(src, dst, intrinsics, dist_coeffs)
cv.SaveImage(output, dst)
def stereoRectify2(self, im2, im2_r):
cv.Remap(im2,
im2_r,
self.rect_map2x,
self.rect_map2y,
flags=cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS,
fillval=0)
def remap(self, src):
"""
:param src: source image
:type src: :class:`cvMat`
Apply the post-calibration undistortion to the source image
"""
r = cv.CloneMat(src)
cv.Remap(src, r, self.mapx, self.mapy)
return r
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 rectifyImage(self, raw, rectified):
"""
:param raw: input image
:type raw: :class:`CvMat` or :class:`IplImage`
:param rectified: rectified output image
:type rectified: :class:`CvMat` or :class:`IplImage`
Applies the rectification specified by camera parameters :math:`K` and and :math:`D` to image `raw` and writes the resulting image `rectified`.
"""
self.mapx = cv.CreateImage((self.width, self.height), cv.IPL_DEPTH_32F, 1)
self.mapy = cv.CreateImage((self.width, self.height), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortRectifyMap(self.K, self.D, self.R, self.P, self.mapx, self.mapy)
cv.Remap(raw, rectified, self.mapx, self.mapy)
def calibrateFromObservations(self,
observations,
calibextrinsic=True,
pruneoutliers=True,
full_output=True,
fixprincipalpoint=False,
Tcameraestimate=None):
while True:
if len(observations) < 3:
raise self.CalibrationError('very little observations: %d!' %
len(observations),
action=1)
object_points = self.getObjectPoints()
all_object_points = []
all_corners = []
imagesize = None
for o in observations:
all_object_points.append(object_points)
all_corners.append(o['corners_raw'])
if imagesize:
assert imagesize[0] == o['image_raw'].shape[
1] and imagesize[1] == o['image_raw'].shape[0]
else:
imagesize = (o['image_raw'].shape[1],
o['image_raw'].shape[0])
intrinsiccondition = self.validateCalibrationData(
all_object_points, all_corners)
if intrinsiccondition is None:
raise self.CalibrationError(
'bad condition number, %d observations' %
(len(observations)),
action=1)
KKorig, kcorig, Traws, error_intrinsic, error_grad = self.calibrateIntrinsicCamera(
all_object_points,
all_corners,
imagesize,
fixprincipalpoint=fixprincipalpoint,
computegradients=True)
cvKKorig = cv.fromarray(KKorig)
cvkcorig = cv.fromarray(kcorig)
thresh = median(error_intrinsic) + 0.5
if any(error_intrinsic > thresh):
# compute again using a pruned set of observations
print 'pruning observations (thresh=%f) because intrinsic error is: ' % thresh, error_intrinsic
observations = [
o for i, o in enumerate(observations)
if error_intrinsic[i] <= thresh
]
else:
if mean(error_intrinsic) > 0.6:
raise self.CalibrationError(
'intrinsic error is huge (%s)! giving up, KK=(%s)' %
(str(error_intrinsic), str(KKorig)))
break
if not calibextrinsic:
return KKorig, kcorig, None, {
'error_intrinsic': error_intrinsic,
'intrinsiccondition': intrinsiccondition,
'error_grad': error_grad
}
if Tcameraestimate is None:
raise TypeError(
'Tcameraestimate needs to be initialized to a 4x4 matrix')
# unwarp all images and re-detect the checkerboard patterns
cvKK = cv.CreateMat(3, 3, cv.CV_64F)
cv.GetOptimalNewCameraMatrix(cvKKorig, cvkcorig, imagesize, 0, cvKK)
cvUndistortionMapX = cv.CreateMat(imagesize[1], imagesize[0],
cv.CV_32F)
cvUndistortionMapY = cv.CreateMat(imagesize[1], imagesize[0],
cv.CV_32F)
cv.InitUndistortRectifyMap(cvKKorig, cvkcorig, cv.fromarray(eye(3)),
cvKK, cvUndistortionMapX,
cvUndistortionMapY)
KK = array(cvKK)
KKinv = linalg.inv(KK)
if full_output:
print 'KKorig: ', KKorig, ' KK:', KK
cvimage = None
cvkczero = cv.fromarray(zeros(kcorig.shape))
cvrvec = cv.CreateMat(1, 3, cv.CV_64F)
cvtvec = cv.CreateMat(1, 3, cv.CV_64F)
all_optimization_data = []
Tworldpatternestimates = []
for i, o in enumerate(observations):
cvimage_dist = cv.fromarray(o['image_raw'])
if not cvimage:
cvimage = cv.CloneMat(cvimage_dist)
cv.Remap(cvimage_dist, cvimage, cvUndistortionMapX,
cvUndistortionMapY,
cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS)
corners = self.detect(cvimage)
if corners is not None:
cv.FindExtrinsicCameraParams2(cv.fromarray(object_points),
cv.fromarray(corners), cvKK,
cvkczero, cvrvec, cvtvec)
T = matrixFromAxisAngle(array(cvrvec)[0])
T[0:3, 3] = array(cvtvec)[0]
Tworldpatternestimates.append(
dot(dot(o['Tlink'], Tcameraestimate), T))
all_optimization_data.append(
(transformPoints(KKinv, corners), linalg.inv(o['Tlink'])))
else:
print 'could not detect original pattern ', i
# have the following equation: Tlink * Tcamera * Tdetectedpattern * corners3d = Tworldpattern * corners3d
# need to solve for Tcamera and Tworldpattern
# instead of using Tdetectedpattern, use projected difference:
# corners - proj( inv(Tcamera) * inv(Tlink) * Tworldpattern *corners3d)
corners3d = self.getObjectPoints()
quatWorldPatternEstimates = array([
quatFromRotationMatrix(T[0:3, 0:3]) for T in Tworldpatternestimates
])
quatWorldPatternInitial, success = leastsq(
lambda q: quatArrayTDist(q / sqrt(sum(q**2)),
quatWorldPatternEstimates),
quatWorldPatternEstimates[0],
maxfev=100000,
epsfcn=1e-6)
rWorldPatternInitial = zeros(6, float64)
rWorldPatternInitial[0:3] = axisAngleFromQuat(quatWorldPatternInitial)
rWorldPatternInitial[3:6] = mean(
array([T[0:3, 3] for T in Tworldpatternestimates]), 0)
Tcameraestimateinv = linalg.inv(Tcameraestimate)
rCameraInitial = zeros(6, float64)
rCameraInitial[0:3] = axisAngleFromRotationMatrix(
Tcameraestimateinv[0:3, 0:3])
rCameraInitial[3:6] = Tcameraestimateinv[0:3, 3]
def errorfn(x, optimization_data):
Tworldpattern = matrixFromAxisAngle(x[0:3])
Tworldpattern[0:3, 3] = x[3:6]
Tcamerainv = matrixFromAxisAngle(x[6:9])
Tcamerainv[0:3, 3] = x[9:12]
err = zeros(len(optimization_data) * len(corners3d) * 2)
off = 0
for measuredcameracorners, Tlinkinv in optimization_data:
cameracorners3d = transformPoints(
dot(dot(Tcamerainv, Tlinkinv), Tworldpattern), corners3d)
iz = 1.0 / cameracorners3d[:, 2]
err[off:(
off + len(corners3d)
)] = measuredcameracorners[:, 0] - cameracorners3d[:, 0] * iz
off += len(corners3d)
err[off:(
off + len(corners3d)
)] = measuredcameracorners[:, 1] - cameracorners3d[:, 1] * iz
off += len(corners3d)
if full_output:
print 'rms: ', sqrt(sum(err**2))
return err
optimization_data = all_optimization_data
xorig, cov_x, infodict, mesg, iter = leastsq(
lambda x: errorfn(x, all_optimization_data),
r_[rWorldPatternInitial, rCameraInitial],
maxfev=100000,
epsfcn=1e-6,
full_output=1)
if pruneoutliers:
# prune the images with the most error
errors = reshape(
errorfn(xorig, all_optimization_data)**2,
(len(all_optimization_data), len(corners3d) * 2))
errorreprojection = mean(
sqrt(KK[0, 0]**2 * errors[:, 0:len(corners3d)] +
KK[1, 1]**2 * errors[:, len(corners3d):]), 1)
#thresh=mean(errorreprojection)+std(errorreprojection)
thresh = median(
errorreprojection) + 20 #0.5*std(errorreprojection)
print 'thresh:', thresh, 'errors:', errorreprojection
optimization_data = [
all_optimization_data[i]
for i in flatnonzero(errorreprojection <= thresh)
]
x, cov_x, infodict, mesg, iter = leastsq(
lambda x: errorfn(x, optimization_data),
xorig,
maxfev=100000,
epsfcn=1e-6,
full_output=1)
else:
x = xorig
Tcamerainv = matrixFromAxisAngle(x[6:9])
Tcamerainv[0:3, 3] = x[9:12]
Tcamera = linalg.inv(Tcamerainv)
Tworldpattern = matrixFromAxisAngle(x[0:3])
Tworldpattern[0:3, 3] = x[3:6]
points3d = self.Compute3DObservationPoints(Tcamerainv,
optimization_data)
if full_output:
errors = reshape(
errorfn(x, optimization_data)**2,
(len(optimization_data), len(corners3d) * 2))
error_reprojection = sqrt(
KK[0, 0]**2 * errors[:, 0:len(corners3d)] +
KK[1, 1]**2 * errors[:, len(corners3d):]).flatten()
print 'final reprojection error (pixels): ', mean(
error_reprojection), std(error_reprojection)
error_3d = sqrt(
sum((transformPoints(Tworldpattern, corners3d) - points3d)**2,
1))
print '3d error: ', mean(error_3d), std(error_3d)
return KKorig, kcorig, Tcamera, {
'error_reprojection': error_reprojection,
'error_3d': error_3d,
'error_intrinsic': error_intrinsic,
'intrinsiccondition': intrinsiccondition,
'error_grad': error_grad,
'KK': array(cvKK)
}
return KKorig, kcorig, Tcamera, None
intrinsics[1, 2] = float(3288 / 2)
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = float(-1)
dist_coeffs[0, 1] = float(-1) #loat(0.0193617)
dist_coeffs[0, 2] = float(-.1) #float(-0.002004)
dist_coeffs[0, 3] = float(-.1) #float(-0.002056)
#End Camera Matrix
allFiles = []
for root, dirs, files in os.walk(startdir + "/"):
allFiles += [os.path.join(root, name) for name in files if ".jpg" in name]
for im in allFiles:
#src = "2015-03-07 11.07.16.jpg"
src = cv.LoadImage(im)
size = cv.GetSize(src)
s = (int(size[0] * 0.9), int(size[1] * 0.9))
res = cv.CreateImage(s, src.depth, src.nChannels)
im1, im2 = cv.CreateImage(s, cv.IPL_DEPTH_32F,
1), cv.CreateImage(s, cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, dist_coeffs, im1, im2)
cv.Remap(src, res, im1, im2, cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS,
cv.ScalarAll(0))
print im.strip(".jpg") + "_nodistort.jpg"
cv.SaveImage(im.strip(".jpg") + "_nodistort.jpg", res)
def undistort_frame(self):
img = self.convert_color()
cv.Remap(img, self.undistort_image, self.undistort_mapx,
self.undistort_mapy, cv.CV_INTER_LINEAR, cv.ScalarAll(0))
return self.undistort_image
def extract_panorama(panopath, outbase, panoinfo, detail):
"""Generates raster, plane, and depth views at rot_degrees"""
print "Processing panorama " + '%d' % panoinfo['pano'][0]
# panopath: location of raster, depth and plane_pano images
# outbase: base name to put generated view, depth, and plane images
# panoinfo: Contains information about the panoramic scene
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# pano and view details details
orientation = [
panoinfo['yaw'][0], panoinfo['pitch'][0], panoinfo['roll'][0]
]
Rpano = geom.RfromYPR(orientation[0], orientation[1], orientation[2])
Kview = geom.cameramat(width, height, fov * pi / 180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM(os.path.join(panopath, 'raster.jpg'),
cv.CV_LOAD_IMAGE_UNCHANGED)
cvDP = cv.fromarray(
np.asarray(Image.open(os.path.join(panopath, 'depth.png'))))
pp = np.asarray(Image.open(os.path.join(panopath,
'plane_pano.png'))).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp == gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(
np.array(np.meshgrid(range(width), range(height))).reshape(2, -1),
np.ones([1, width * height]), 0)
# Create output openCV matrices
cvI = cv.CreateMat(height, width, cv.CV_8UC3)
cvD = cv.CreateMat(height, width, cv.CV_32SC1)
cvP = cv.CreateMat(height, width, cv.CV_8UC1)
for viewdir in [2, 3, 4, 8, 9, 10]:
# add to base name and generate info file
viewname = outbase + '%04d' % viewdir
gen_info_file(panoinfo, viewname + '.info', detail, 30 * viewdir)
# generate view orientation
Yview = np.mod(orientation[0] + 30 * viewdir, 360)
Rview = geom.RfromYPR(Yview, 0, 0)
# compute mappings
ray = np.dot(np.transpose(Rpano), np.dot(Rview, np.dot(Kinv, pix)))
yaw, pitch = np.arctan2(ray[0, :], ray[2, :]), np.arctan2(
-ray[1, :], np.sqrt((np.array([ray[0, :], ray[2, :]])**2).sum(0)))
ix, iy = cv.fromarray(
np.array(8192 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(4096 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
dx, dy = cv.fromarray(
np.array(5000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(2500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
px, py = cv.fromarray(
np.array(1000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
# call remap function
cv.Remap(cvIP, cvI, ix, iy, cv.CV_INTER_CUBIC
) # if detail else cv.Remap(cvIP,cvI,ix,iy,cv.CV_INTER_AREA)
cv.Remap(cvDP, cvD, dx, dy, cv.CV_INTER_NN)
cv.Remap(cvPP, cvP, px, py, cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:, :,
[2, 1, 0]]).save(viewname + '.jpg',
'jpeg')
Image.fromarray(np.array(cvD)).save(viewname + '-depth.png', 'png')
Image.fromarray(np.array(cvP)).save(viewname + '-planes.png', 'png')
def genView(panopath, outpath, orientation, viewdir, detail):
# panopath: location of raster, depth and plane_pano images
# outpath: location to put generated view, depth, and plane images
# orientation: [yaw, pitch, roll] of panorama (in degrees)
# viewdir: clock-based view; in set [2,3,4,8,9,10] for database
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
start = time.time()
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# view details
Rpano = geom.RfromYPR(orientation[0], orientation[1], orientation[2])
Yview = np.mod(orientation[0] + 30 * viewdir, 360)
Rview = geom.RfromYPR(Yview, 0, 0)
Kview = geom.cameramat(width, height, fov * pi / 180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM(os.path.join(panopath, 'raster.jpg'),
cv.CV_LOAD_IMAGE_UNCHANGED)
cvDP = cv.fromarray(
np.asarray(Image.open(os.path.join(panopath, 'depth.png'))))
pp = np.asarray(Image.open(os.path.join(panopath,
'plane_pano.png'))).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp == gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(
np.array(np.meshgrid(range(width), range(height))).reshape(2, -1),
np.ones([1, width * height]), 0)
midpoint = time.time()
print 'Loading pano images took ' + str(midpoint - start) + ' seconds.'
# Create output openCV matrices
cvI = cv.CreateMat(height, width, cv.CV_8UC3)
cvD = cv.CreateMat(height, width, cv.CV_32SC1)
cvP = cv.CreateMat(height, width, cv.CV_8UC1)
# compute mappings
ray = np.dot(np.transpose(Rpano), np.dot(Rview, np.dot(Kinv, pix)))
yaw, pitch = np.arctan2(ray[0, :], ray[2, :]), np.arctan2(
-ray[1, :], np.sqrt((np.array([ray[0, :], ray[2, :]])**2).sum(0)))
ix, iy = cv.fromarray(
np.array(8192 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(4096 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
dx, dy = cv.fromarray(
np.array(5000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(2500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
px, py = cv.fromarray(
np.array(1000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
# call remap function
cv.Remap(cvIP, cvI, ix, iy, cv.CV_INTER_CUBIC)
cv.Remap(cvDP, cvD, dx, dy, cv.CV_INTER_NN)
cv.Remap(cvPP, cvP, px, py, cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:, :, [2, 1, 0]]).save(
os.path.join(outpath, 'view.jpg'), 'jpeg')
Image.fromarray(np.array(cvD)).save(os.path.join(outpath, 'depth.png'),
'png')
Image.fromarray(np.array(cvP)).save(os.path.join(outpath, 'plane.png'),
'png')
print 'Generating views from pano took ' + str(time.time() -
midpoint) + ' seconds.'
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)
cv.ShowImage("Undistort", image)
c = cv.WaitKey(15)
if (c == ord('p')):
c = 0
while (c != ord('p') and c != 27):
c = cv.WaitKey(250)
if (c == 27):
break
image = cv.QueryFrame(cam)
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 VirtualMirror():
cv.NamedWindow("RGB_remap", cv.CV_WINDOW_NORMAL)
cv.NamedWindow("Depth_remap", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow('dst', cv.CV_WINDOW_NORMAL)
cv.SetMouseCallback("Depth_remap", on_mouse, None)
print "Virtual Mirror"
print "Calibrated 4 Screen corner= ", sn4_ref
print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0] - sn4_ref[1])
print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1] - sn4_ref[2])
print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2] - sn4_ref[3])
print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3] - sn4_ref[0])
global head_pos
global head_virtual
global scene4_cross
head_pos = np.array([-0.2, -0.2, 1.0]) #Head_detect()
while 1:
(depth, _) = freenect.sync_get_depth()
(rgb, _) = freenect.sync_get_video()
#print type(depth)
img = array2cv(rgb[:, :, ::-1])
im = array2cv(depth.astype(np.uint8))
#modulize this part for update_on() and loopcv()
#q = depth
X, Y = np.meshgrid(range(640), range(480))
d = 2 #downsampling if need
projpts = calibkinect.depth2xyzuv(depth[::d, ::d], X[::d, ::d],
Y[::d, ::d])
xyz, uv = projpts
if tracking == 0:
#*********************************
if pt is not None:
print "=================="
(x_d, y_d) = pt
print "x=", x_d, " ,y=", y_d
#print depth.shape
#Watch out the indexing for depth col,row = 480,640
d_raw = np.array([depth[y_d, x_d]])
u_d = np.array([x_d])
v_d = np.array([y_d])
print "d_raw= ", d_raw
print "u_d= ", u_d
print "v_d= ", v_d
head3D, head2D = calibkinect.depth2xyzuv(d_raw, u_d, v_d)
print "XYZ=", head3D
print "XYZonRGBplane=", head2D
head_pos = head3D[0]
#print "head_pos.shape",head_pos.shape
print "head_pos= ", head_pos
cv.WaitKey(100)
cv.Circle(im, (x_d, y_d), 4, (0, 0, 255, 0), -1, 8, 0)
cv.Circle(im, (int(head2D[0, 0]), int(head2D[0, 1])), 2,
(255, 255, 255, 0), -1, 8, 0)
#*********************************
elif tracking == 1:
#find the nearest point (nose) as reference for right eye position
print "nose"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :] + (0.03, 0.04, 0.01)
#print head_pos.shape
#print head_pos
elif tracking == 2:
#find the closest point as eye posiiton
print "camera"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :]
#print head_pos.shape
#print head_pos
else:
print "please select a tracking mode"
head_virtual = MirrorReflection(sn4_ref[0:3, :], head_pos)
print "head_virtual= ", head_virtual
rgbK = np.array([[520.97092069697146, 0.0, 318.40565581396697],
[0.0, 517.85544366622719, 263.46756370601804],
[0.0, 0.0, 1.0]])
rgbD = np.array([[0.22464481251757576], [-0.47968370787671893], [0.0],
[0.0]])
irK = np.array([[588.51686020601733, 0.0, 320.22664144213843],
[0.0, 584.73028132692866, 241.98395817513071],
[0.0, 0.0, 1.0]])
irD = np.array([[-0.1273506872313161], [0.36672476189160591], [0.0],
[0.0]])
mapu = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapv = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(rgbK, rgbD, mapu, mapv)
cv.InitUndistortMap(irK, irD, mapx, mapy)
if 1:
rgb_remap = cv.CloneImage(img)
cv.Remap(img, rgb_remap, mapu, mapv)
depth_remap = cv.CloneImage(im)
cv.Remap(im, depth_remap, mapx, mapy)
scene4_cross = Cross4Pts.CrossPts(xyz, uv, head_pos, head_virtual,
sn4_ref)
#[warp] Add whole warpping code here
#[warp] points = Scene4Pts() as warpping 4 pts
#Flip the dst image!!!!!!!!!
#ShowImage("rgb_warp", dst)
#Within/out of the rgb range
#Mapping Destination (width, height)=(x,y)
#Warning: the order of pts in clockwise: pt1(L-T),pt2(R-T),pt3(R-B),pt4(L-B)
#points = [(test[0,0],test[0,1]), (630.,300.), (700.,500.), (400.,470.)]
points = [(scene4_cross[0, 0], scene4_cross[0, 1]),
(scene4_cross[1, 0], scene4_cross[1, 1]),
(scene4_cross[2, 0], scene4_cross[2, 1]),
(scene4_cross[3, 0], scene4_cross[3, 1])]
#Warping the image without flipping (camera image)
#npoints = [(0.,0.), (640.,0.), (640.,480.), (0.,480.)]
#Warping the image with flipping (mirror flip image)
npoints = [(640., 0.), (0., 0.), (0., 480.), (640., 480.)]
mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(points, npoints, mat)
#src = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_32F, 3 )
src = cv.CreateImage(cv.GetSize(rgb_remap), cv.IPL_DEPTH_32F, 3)
#cv.ConvertScale(img,src,(1/255.00))
cv.ConvertScale(rgb_remap, src, (1 / 255.00))
dst = cv.CloneImage(src)
cv.Zero(dst)
cv.WarpPerspective(src, dst, mat)
#************************************************************************
#Remap the rgb and depth image
#Warping will use remap rgb image as src
if 1:
cv.ShowImage("RGB_remap", rgb_remap) #rgb[200:440,300:600,::-1]
cv.ShowImage("Depth_remap", depth_remap)
cv.ShowImage("dst", dst) #warp rgb image
if cv.WaitKey(5) == 27:
cv.DestroyWindow("RGB_remap")
cv.DestroyWindow("Depth_remap")
cv.DestroyWindow("dst")
break
exT = cv.CreateMat(len(goodcorners), 3, cv.CV_32FC1)
# focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(
opts,
ipts,
npts,
cv.GetSize(images[0]),
intrinsics,
distortion,
exR,
exT,
flags=cv.CV_CALIB_ZERO_TANGENT_DIST) #cv.CV_CALIB_ZERO_TANGENT_DIST) # 0)
print "D =", list(cvmat_iterator(distortion))
print "K =", list(cvmat_iterator(intrinsics))
print "R =", list(cvmat_iterator(exR))
print "T =", list(cvmat_iterator(exT))
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
for img in images:
r = cv.CloneMat(img)
cv.Remap(img, r, mapx, mapy)
cv.ShowImage("snap", r)
cv.WaitKey()
def undistort(image):
result = cv.CreateImage(cv.GetSize(image), image.depth, image.nChannels)
cv.Remap(image, result, mapX, mapY,
cv.CV_INTER_CUBIC + cv.CV_WARP_FILL_OUTLIERS, cv.ScalarAll(0))
return result
old_time = time.time()
intrinsics = cv.Load("Intrinsics.xml")
distortion = cv.Load("Distortion.xml")
image = cv.QueryFrame(cam)
mapx = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(intrinsics, distortion, mapx, mapy)
while (True):
start = time.time()
image = cv.QueryFrame(cam)
processed = cv.CloneImage(image)
cv.Remap(image, processed, mapx, mapy)
crop_rect = (0, 63, 640, 350)
cv.SetImageROI(processed, crop_rect)
ball_center = find_object(processed, "RED")
ball_center = (int(ball_center[0][0]) + 8, int(ball_center[0][1]) + 8)
blue_center = find_object(processed, "BLUE")
blue_center = ((int(blue_center[0][0]) + 8, int(blue_center[0][1]) + 12),
blue_center[1])
yellow_center = find_object(processed, "YELLOW")
yellow_center = ((int(yellow_center[0][0]) + 8,
int(yellow_center[0][1]) + 12), yellow_center[1])
center_points = (ball_center, blue_center, yellow_center)
draw_on_image(processed, center_points)
