def make_cam_from_params(self, params):
if 1:
t = params[:3]
rod = params[3:]
rmat = rodrigues2matrix( rod )
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = rmat
cam_model = CameraModel.from_dict(d)
return cam_model
C = params[:3]
quat = params[3:]
qmag = np.sqrt(np.sum(quat**2))
quat = quat/qmag
R,rquat=get_rotation_matrix_and_quaternion(quat)
t = -np.dot(R, C)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = R
cam_model = CameraModel.from_dict(d)
return cam_model
def make_cam_from_params(self, params):
if 1:
t = params[:3]
rod = params[3:]
rmat = rodrigues2matrix(rod)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = rmat
cam_model = CameraModel.from_dict(d)
return cam_model
C = params[:3]
quat = params[3:]
qmag = np.sqrt(np.sum(quat**2))
quat = quat / qmag
R, rquat = get_rotation_matrix_and_quaternion(quat)
t = -np.dot(R, C)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = R
cam_model = CameraModel.from_dict(d)
return cam_model
def generate_camera(self):
(width,height)=(self.width,self.height)=(640,480)
center = 1,2,3
rot_axis = np.array((4,5,6.7))
rot_axis = rot_axis / np.sum(rot_axis**2)
rquat = tf.transformations.quaternion_about_axis(0.1, (rot_axis.tolist()))
rmat,_ = get_rotation_matrix_and_quaternion(rquat)
parts = make_M( 1234.56, width, height,
rmat, center)
if self.use_distortion:
dist = [-0.4, .2, 0, 0, 0]
else:
dist = [0, 0, 0, 0, 0]
self.cam = CameraModel.load_camera_from_M(parts['M'],
width=width,height=height,
distortion_coefficients=dist)
def generate_images(self):
"""make checkerboard images in camera view"""
max_theta = 100.0*D2R
axis = (0,1,0)
self.check_w = 8
self.check_h = 6
checkerboard_w = self.check_w+2
checkerboard_h = self.check_h+2
self.check_size = 0.024
base_cc_x=(np.arange(checkerboard_w)-checkerboard_w/2.0)*self.check_size
base_cc_y=(np.arange(checkerboard_h)-checkerboard_h/2.0)*self.check_size
base_cc = []
save_idx = []
for i,y in enumerate(base_cc_y):
for j,x in enumerate(base_cc_x):
if (i>0) and (i<checkerboard_h-1):
if (j>0) and (j<checkerboard_w-1):
# save indices of actual checkerboard corners
save_idx.append(len(base_cc))
base_cc.append( (x,y,0) )
save_idx = np.array(save_idx)
base_cc = np.array(base_cc).T
self.db = []
center_pix = (self.cam.width/2.0, self.cam.height/2.0)
n_images = 20
for i in range(n_images):
dist = 0.9 + 0.1*(i%3)
theta = i/float(n_images-1)*max_theta - max_theta*0.5
rquat = tf.transformations.quaternion_about_axis(theta, axis)
rmat,_ = get_rotation_matrix_and_quaternion(rquat)
this_cc = np.dot(rmat,base_cc)
first_pixel = np.array( center_pix, copy=True )
atmp = i*np.pi/2.
dir_offset = np.array((np.cos(atmp), np.sin(atmp)))
offset = dir_offset*40.0
first_pixel += offset
first_pixel.shape = (1,2)
first_3d = self.cam.project_pixel_to_3d_ray(first_pixel,
distorted=True,
distance=dist )
check_3d = this_cc.T + first_3d
check_pixels = self.cam.project_3d_to_pixel(check_3d,distorted=True)
im = draw_checkerboard(check_pixels,checkerboard_w,checkerboard_h,
self.cam.width,self.cam.height)
imsave = np.empty( (self.cam.height, self.cam.width, 3),
dtype=np.uint8)
for chan in range(3):
imsave[:,:,chan] = (im*255).astype(np.uint8)
wcs3d = check_3d[save_idx] # world coords
ccs3d = np.dot( self.cam.get_rotation(), wcs3d.T ).T + self.cam.translation
ccs2d = check_pixels[save_idx] # pixel coords
if DRAW:
import scipy.misc
scipy.misc.imsave( 'im%03d.png'%i, imsave )
self.db.append( {'wc':wcs3d, 'cc':ccs3d, 'pix':ccs2d, 'im':imsave })
