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)