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 getCameraWidth(self, alt):
'''Using the camera parameters, with the width of the
ground strip that the camera can see from a particular altitude'''
#use the camera parameters
c_params = CameraParams(lens=4.0)
# load camera params and get the width of an image on the ground
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', 'chameleon1_arecont0.json')
c_params.load(path)
aov = math.degrees(2.0*math.atan((c_params.sensorwidth/1000.0)/(2.0*c_params.lens/1000.0)))
groundWidth = 2.0*alt*math.tan(math.radians(aov/2))
return groundWidth
def __init__(self):
self.running = False
self.unload = threading.Event()
self.unload.clear()
self.capture_thread = None
self.save_thread = None
self.scan_thread1 = None
self.scan_thread2 = None
self.transmit_thread = None
self.view_thread = None
self.settings = mp_settings.MPSettings([('depth', int, 8),
('gcs_address', str, None),
('gcs_view_port', int, 7543),
('bandwidth', int, 40000),
('bandwidth2', int, 2000),
('capture_brightness', int,
150), ('gamma', int, 950),
('brightness', float, 1.0),
('quality', int, 75),
('save_pgm', int, 1),
('transmit', int, 1),
('roll_stabilised', int, 1),
('minscore', int, 75),
('minscore2', int, 500),
('altitude', int, None),
('send1', int, 1),
('send2', int, 1),
('maxqueue1', int, None),
('maxqueue2', int, 30),
('thumbsize', int, 60),
('packet_loss', int, 0),
('gcs_slave', str, None)])
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.fps = 0
self.scan_fps = 0
self.cam = None
self.save_queue = Queue.Queue()
self.scan_queue = Queue.Queue()
self.transmit_queue = Queue.Queue()
self.viewing = False
self.c_params = CameraParams(lens=4.0)
self.jpeg_size = 0
self.xmit_queue = 0
self.xmit_queue2 = 0
self.efficiency = 1.0
self.last_watch = 0
self.frame_loss = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = 0
self.rtt_estimate = 0
self.bsocket = None
self.bsend2 = None
self.bsend_slave = None
# setup directory for images
self.camera_dir = os.path.join(os.path.dirname(mpstate.logfile_name),
"camera")
cuav_util.mkdir_p(self.camera_dir)
self.mpos = mav_position.MavInterpolator(backlog=5000, gps_lag=0.3)
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, 'joe.log'),
append=mpstate.continue_mode)
# load camera params
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..',
'..', '..', 'cuav', 'data',
'chameleon1_arecont0.json')
self.c_params.load(path)
def __init__(self):
self.running = False
self.unload = threading.Event()
self.unload.clear()
self.capture_thread = None
self.save_thread = None
self.scan_thread1 = None
self.scan_thread2 = None
self.transmit_thread = None
self.view_thread = None
self.settings = mp_settings.MPSettings(
[ ('depth', int, 8),
('gcs_address', str, None),
('gcs_view_port', int, 7543),
('bandwidth', int, 40000),
('bandwidth2', int, 2000),
('capture_brightness', int, 150),
('gamma', int, 950),
('brightness', float, 1.0),
('quality', int, 75),
('save_pgm', int, 1),
('transmit', int, 1),
('roll_stabilised', int, 1),
('minscore', int, 75),
('minscore2', int, 500),
('altitude', int, None),
('send1', int, 1),
('send2', int, 1),
('maxqueue1', int, None),
('maxqueue2', int, 30),
('thumbsize', int, 60),
('packet_loss', int, 0)
]
)
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.fps = 0
self.scan_fps = 0
self.cam = None
self.save_queue = Queue.Queue()
self.scan_queue = Queue.Queue()
self.transmit_queue = Queue.Queue()
self.viewing = False
self.c_params = CameraParams(lens=4.0)
self.jpeg_size = 0
self.xmit_queue = 0
self.xmit_queue2 = 0
self.efficiency = 1.0
self.last_watch = 0
self.frame_loss = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = 0
self.rtt_estimate = 0
self.bsocket = None
self.bsend2 = None
# setup directory for images
self.camera_dir = os.path.join(os.path.dirname(mpstate.logfile_name),
"camera")
cuav_util.mkdir_p(self.camera_dir)
self.mpos = mav_position.MavInterpolator(backlog=5000, gps_lag=0.3)
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, 'joe.log'), append=mpstate.continue_mode)
# load camera params
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..',
'cuav', 'data', 'chameleon1_arecont0.json')
self.c_params.load(path)
def __init__(self):
self.running = False
self.unload = threading.Event()
self.unload.clear()
self.capture_thread = None
self.save_thread = None
self.scan_thread1 = None
#self.scan_thread2 = None
self.transmit_thread = None
self.view_thread = None
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.fps = 0
self.scan_fps = 0
self.cam = None
self.save_queue = Queue.Queue()
self.scan_queue = Queue.Queue()
self.transmit_queue = Queue.Queue()
self.viewing = False
self.depth = 8
self.gcs_address = None
self.gcs_view_port = 7543
self.bandwidth = 40000
self.bandwidth2 = 800
self.capture_brightness = 150
self.gamma = 950
self.c_params = CameraParams(lens=4.0)
self.brightness = 1.0
self.quality = 75
self.jpeg_size = 0
self.xmit_queue = 0
self.xmit_queue2 = 0
self.efficiency = 1.0
self.last_watch = 0
self.frame_loss = 0
self.colour = 1
self.boundary = None
self.boundary_polygon = None
self.packet_loss = 0
self.save_pgm = True
self.bandwidth_used = 0
self.rtt_estimate = 0
self.transmit = True
self.roll_stabilised = True
self.minscore = 3
self.altitude = None
self.bsocket = None
self.bsend2 = None
self.send2 = False
self.thumbsize = 60
# setup directory for images
self.camera_dir = os.path.join(os.path.dirname(mpstate.logfile_name),
"camera")
cuav_util.mkdir_p(self.camera_dir)
self.mpos = mav_position.MavInterpolator(backlog=5000, gps_lag=0.3)
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir, 'joe.log'), append=mpstate.continue_mode)
# load camera params
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..',
'cuav', 'data', 'chameleon1_arecont0.json')
self.c_params.load(path)
def calibrate(imagedir):
nimages = 0
datapoints = []
im_dims = (0,0)
for f in os.listdir(imagedir):
if (f.find('pgm')<0):
continue
image = imagedir+'/'+f
grey = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE)
found,points=cv.FindChessboardCorners(grey,dims,cv.CV_CALIB_CB_ADAPTIVE_THRESH)
points=cv.FindCornerSubPix(grey,points,(11,11),(-1,-1),(cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER,30,0.1))
if (found):
print 'using ', image
nimages += 1
datapoints.append(points)
im_dims = (grey.width, grey.height)
#Number of points in chessboard
num_pts = dims[0] * dims[1]
#image points
ipts = cv.CreateMat(nimages * num_pts, 2, cv.CV_32FC1)
#object points
opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1)
npts = cv.CreateMat(nimages, 1, cv.CV_32SC1)
for i in range(0,nimages):
k=i*num_pts
squareSize = 1.0
# squareSize is 1.0 (i.e. units of checkerboard)
for j in range(num_pts):
cv.Set2D(ipts,k,0,datapoints[i][j][0])
cv.Set2D(ipts,k,1,datapoints[i][j][1])
cv.Set2D(opts,k,0,float(j%dims[0])*squareSize)
cv.Set2D(opts,k,1,float(j/dims[0])*squareSize)
cv.Set2D(opts,k,2,0.0)
k=k+1
cv.Set2D(npts,i,0,num_pts)
K = cv.CreateMat(3, 3, cv.CV_64FC1)
D = cv.CreateMat(5, 1, cv.CV_64FC1)
cv.SetZero(K)
cv.SetZero(D)
# focal lengths have 1/1 ratio
K[0,0] = im_dims[0]
K[1,1] = im_dims[0]
K[0,2] = im_dims[0]/2
K[1,2] = im_dims[1]/2
K[2,2] = 1.0
rcv = cv.CreateMat(nimages, 3, cv.CV_64FC1)
tcv = cv.CreateMat(nimages, 3, cv.CV_64FC1)
#print 'object'
#print array(opts)
#print 'image'
#print array(ipts)
#print 'npts'
#print array(npts)
size=cv.GetSize(grey)
flags = 0
#flags |= cv.CV_CALIB_FIX_ASPECT_RATIO
#flags |= cv.CV_CALIB_USE_INTRINSIC_GUESS
#flags |= cv.CV_CALIB_ZERO_TANGENT_DIST
#flags |= cv.CV_CALIB_FIX_PRINCIPAL_POINT
cv.CalibrateCamera2(opts, ipts, npts, size, K, D, rcv, tcv, flags)
# storing results using CameraParams
C = CameraParams(xresolution=im_dims[0], yresolution=im_dims[1])
print array(K)
print array(D)
C.setParams(K, D)
C.save(imagedir+"/params.json")
def calibrate(imagedir, autodelete=True, validate=True):
nimages = 0
datapoints = []
im_dims = (0,0)
im_cnt = 0
for f in os.listdir(imagedir):
if (f.find('pgm')<0):
continue
image = imagedir+'/'+f
grey = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE)
found,points=cv.FindChessboardCorners(grey,dims,cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if found == 0:
print 'Failed to find corners.'
if (autodelete):
print 'Deleting image: ' + f
os.remove(image)
else:
print 'Skipping image: ' + f
elif validate:
grey_c = cv.CloneImage(grey)
cv.DrawChessboardCorners(grey_c,dims,points,found)
#show the final image
if (grey_c.width > 640):
tmp=cv.CreateImage((grey_c.width/2,grey_c.height/2),8,grey_c.channels)
cv.Resize(grey_c,tmp)
cv.ShowImage("calibrate", tmp)
else:
cv.ShowImage("calibrate", gray_c)
#wait indefinitely
print 'Use this image/quit ? (y)es/(n)o/(d)elete/(q)uit'
key = chr(cv.WaitKey(0) & 255)
if (key == 'y'):
print 'Keeping image: ' + f + ' : ', im_cnt
im_cnt+=1
elif (key == 'q'):
break
elif (key == 'd'):
print 'Deleting original image: ' + f
os.remove(image)
continue
else:
print 'Skipping image: ' + f
continue
points=cv.FindCornerSubPix(grey,points,(11,11),(-1,-1),(cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER,30,0.1))
if (found):
print 'using ', image
nimages += 1
datapoints.append(points)
im_dims = (grey.width, grey.height)
#Number of points in chessboard
num_pts = dims[0] * dims[1]
#image points
ipts = cv.CreateMat(nimages * num_pts, 2, cv.CV_32FC1)
#object points
opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1)
npts = cv.CreateMat(nimages, 1, cv.CV_32SC1)
for i in range(0,nimages):
k=i*num_pts
squareSize = 1.0
# squareSize is 1.0 (i.e. units of checkerboard)
for j in range(num_pts):
cv.Set2D(ipts,k,0,datapoints[i][j][0])
cv.Set2D(ipts,k,1,datapoints[i][j][1])
cv.Set2D(opts,k,0,float(j%dims[0])*squareSize)
cv.Set2D(opts,k,1,float(j/dims[0])*squareSize)
cv.Set2D(opts,k,2,0.0)
k=k+1
cv.Set2D(npts,i,0,num_pts)
K = cv.CreateMat(3, 3, cv.CV_64FC1)
D = cv.CreateMat(5, 1, cv.CV_64FC1)
cv.SetZero(K)
cv.SetZero(D)
# focal lengths have 1/1 ratio
K[0,0] = im_dims[0]
K[1,1] = im_dims[0]
K[0,2] = im_dims[0]/2
K[1,2] = im_dims[1]/2
K[2,2] = 1.0
rcv = cv.CreateMat(nimages, 3, cv.CV_64FC1)
tcv = cv.CreateMat(nimages, 3, cv.CV_64FC1)
#print 'object'
#print array(opts)
#print 'image'
#print array(ipts)
#print 'npts'
#print array(npts)
size=cv.GetSize(grey)
flags = 0
#flags |= cv.CV_CALIB_FIX_ASPECT_RATIO
#flags |= cv.CV_CALIB_USE_INTRINSIC_GUESS
#flags |= cv.CV_CALIB_ZERO_TANGENT_DIST
#flags |= cv.CV_CALIB_FIX_PRINCIPAL_POINT
cv.CalibrateCamera2(opts, ipts, npts, size, K, D, rcv, tcv, flags)
# storing results using CameraParams
C = CameraParams(xresolution=im_dims[0], yresolution=im_dims[1])
print array(K)
print array(D)
C.setParams(K, D)
C.save(imagedir+"/params.json")
