def test_pose_init(self): p0 = util.Pose(np.array([3,4,5]).reshape(3,1), np.zeros((3,1)), 0) p1 = util.Pose(np.array([3,4,5]), np.zeros((3,1)), 0) p2 = util.Pose(np.array([3,4,5]), np.eye(3), 0) assert( np.allclose(p0.loc, p1.loc)) assert( np.allclose(p0.loc, p2.loc)) assert( np.allclose(p0.ori, p1.ori)) assert( np.allclose(p0.ori, p2.ori))
def robot_pose_from_board_pixel_align(camera, camera_loc, board,
robot2board_pose, depth, pxl,
align_point):
pt3d, ray_vec = camera.ray_from_pixel(pxl, camera_loc.extrinsics())
board_loc = pt3d + depth * ray_vec # board align point location
z_vec = np.array([0, 0, 1]).reshape(3, 1)
board_ori = util.rot_mat_vec2vec(z_vec, -ray_vec) # board orientation
# vis.plot_camera_with_rays(camera_loc.extrinsics(), [(pt3d, -ray_vec), (pt3d, board_ori.dot(z_vec))])
align2base = util.Pose(board_loc, board_ori)
T_align2base = align2base.T_pose2world()
# Calculate board pose from align point pose
tl2align_trans = -board.pts[align_point]
tl2align = util.Pose(tl2align_trans, np.eye(3))
T_tl2align = tl2align.T_pose2world()
# Calculate robot pose from board pose
T_robot2align = robot2board_pose.T_pose2world()
T_robot2base = T_align2base.dot(T_tl2align.dot(T_robot2align))
return util.Pose(T_robot2base[0:3, -1], T_robot2base[0:3, 0:3])
def test_pose_transform(self): p0 = util.Pose(np.zeros((3,1)), np.zeros((3,1)), 0) p1 = util.Pose(np.array([1,0,0]), np.zeros((3,1)), 0) #translation p1_0 = p0.transform_p2w(p1) assert( np.allclose(p1_0.loc, p1.loc) ) assert( np.allclose(p1_0.ori, p1.ori) ) assert( np.allclose(p1_0.time, p1.time) ) p0_1 = p1.transform_p2w(p0) assert( np.allclose(p0_1.loc, p1.loc) ) assert( np.allclose(p0_1.ori, p1.ori) ) assert( np.allclose(p0_1.time, p1.time) ) p2 = util.Pose(np.zeros((3,1)), np.array([math.pi/6, 0, 0])) #rotate 30 degrees in x axis p0_2 = p2.transform_p2w(p0) assert( np.allclose(p0_2.loc, p2.loc) ) assert( np.allclose(p0_2.ori, p2.ori) ) assert( np.allclose(p0_2.time, p1.time) ) p1_2 = p2.transform_p2w(p1) assert( np.allclose(p1_2.loc, p1.loc) ) assert( np.allclose(p1_2.ori, p2.ori) ) assert( np.allclose(p1_2.time, p1.time) ) p2_1 = p1.transform_p2w(p2) assert( np.allclose(p2_1.loc, p1.loc) ) assert( np.allclose(p2_1.ori, p2.ori) ) assert( np.allclose(p2_1.time, p1.time) ) p3 = util.Pose(np.array([0,2,0]), np.zeros((3,1)), time=0.3) #translation and time offset p3_0 = p0.transform_p2w(p3) assert( np.allclose(p3_0.loc, p3.loc) ) assert( np.allclose(p3_0.ori, p3.ori) ) assert( np.allclose(p3_0.time, p3.time) ) p3_1 = p1.transform_p2w(p3) assert( np.allclose(p3_1.loc, np.array([1,2,0]).reshape(3,1)) ) assert( np.allclose(p3_1.ori, p3.ori) ) assert( np.allclose(p3_1.time, p3.time) ) p3_2 = p2.transform_p2w(p3) assert( np.allclose(p3_2.loc, np.array([0, math.sqrt(3), 1]).reshape(3,1)) ) assert( np.allclose(p3_2.ori, p2.ori) ) assert( np.allclose(p3_2.time, p3.time) )
def gen_calib_board(cls, board_dim, sqsize, \
location, orientation, noise3d):
"""
Generate a calibration board placed at give location with given direction,
with given number of points on a plane.
Args:
board_dim: (board_width, board_height), number of control points in
horizontal and vertical direction
sqsize: positive number, size of each square, in m
location: 3x1 numpy array, 3D location of the top-left corner of board
orientation: 3x1 or 3x3 numpy array, 3D orientation of the board,
specified by Euler angles or rotation matrix
noise3d: positive number, standard deviation of Gaussian noise
added to board, in m
Returns:
board: a Board
"""
# Check inputs
assert (location.size == 3)
location = location.reshape(3, 1)
assert (orientation.size == 3 or orientation.size == 9)
if orientation.size == 3:
orientation = orientation.reshape(3, 1)
orientation = 1.0 * orientation
assert (len(board_dim) == 2 and board_dim[0] > 0 and board_dim[1] > 0)
board_pts = {}
# Make board whose top-left point is at (0,0,0) and lies on x-y plane
for x in xrange(board_dim[1]):
for y in xrange(board_dim[0]):
board_pts[(y, x)] = np.array([[x * sqsize, y * sqsize, 0]],
np.float32).T
# Add noise3d
if noise3d > 0:
noises = np.random.normal(0, noise3d,
(board_dim[1] * board_dim[0], 3))
for ipt in xrange(len(board_pts)):
board_pts[board_pts.keys()[ipt]] += noises[ipt, :].reshape(
3, 1)
# # Rotate board to given orientation and move to given location
# if orientation.size == 3:
# rot_mat, _ = cv2.Rodrigues(orientation)
# else:
# rot_mat = orientation
# for pt in board_pts.keys():
# board_pts[pt] = np.dot(rot_mat, board_pts[pt]) + location
orig_pose = util.Pose(location, orientation)
return cls(board_dim, board_pts, orig_pose)
def experiment_intrinsics_calib():
# Setup camera
camera = cam.Camera.make_pinhole_camera()
camera_loc = np.array([0, 2, 0.5]).reshape(3, 1)
camera_ori = np.array([np.pi / 2, 0, 0]).reshape(3, 1)
camera_pose = util.Pose(camera_loc, camera_ori)
# Pick a calibration board
bw = 8
bh = 5
bs = 0.23
board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), 0)
# Mount board to robot arm
robot2board_pose = util.Pose(np.zeros((3, 1)), np.zeros((3, 1)))
samples = (2, 2)
r_poses = robot_calibrate_cam_intrinsics(camera,
camera_pose,
board,
robot2board_pose,
samples,
border_pxl=100)
#print r_poses
obs = []
for p in r_poses:
board = board.move_board(p.loc, p.ori)
obs.append(board.get_points())
vis.plot_camera_with_points(camera_pose.extrinsics(), obs)
plt.show()
img_pts = camera.capture_images(camera_pose.extrinsics(), obs, 0)
vis.plot_all_chessboards_in_camera(
img_pts,
camera.size,
seperate_plot=True,
save_name='results/capture_intrin_calib.pdf')
def move_board(self, location, orientation):
"""
Moves the current board to a new given pose
Args:
location: 3x1 numpy array, desired 3D location of the top-left corner
of board
orientation: 3x1 or 3x3 numpy array, desired 3D orientation of the board,
(rx,ry,rz)
Returns:
A dictionary keyed by point id, whose values are 3D points
"""
# return Board(self.size, self.pts, util.Pose(location, orientation))
self.pose = util.Pose(location, orientation)
return self
def read_motion(filename, sample_ratio=1):
"""
Reads headset motion captured with mocap. All lengths in m.
Args:
filename: csv file to read from, column in order:
time stamp; position x, y, z; orientation x,y,z,w;
linear velocity x, y, z; angular velocity x, y, z;
linear acceleration x, y, z; angular acceleration x, y, z
sample_ratio: read one sample out of sample_ratio rows
Returns:
motion: a list of Poses
"""
motion = []
csvfile = open(filename, 'rb')
reader = csv.reader(csvfile, delimiter=',')
cnt = 0
tcnt = 0
for row in reader:
if cnt == 0: # skip header
cnt += 1
continue
# fast debugging
# if cnt == 3000:
# break
if cnt % sample_ratio == 0:
ts = int(row[0])
loc = np.array([float(row[1]),
float(row[2]),
float(row[3])]).reshape(3, 1)
rot_mat = util.quaternion2mat(float(row[4]), float(row[5]),
float(row[6]), float(row[7]))
lvel = np.array([float(row[8]), float(row[9]), float(row[10])])
avel = np.array([float(row[11]), float(row[12]), float(row[13])])
lacc = np.array([float(row[14]), float(row[15]), float(row[16])])
aacc = np.array([float(row[17]), float(row[18]), float(row[19])])
pose = util.Pose(loc, rot_mat, ts, lvel, avel, lacc, aacc)
motion.append(pose)
tcnt += 1
cnt += 1
print 'read', cnt, 'poses, recorded', tcnt, 'poses'
return motion
def test_ray_from_pixel_moved(self):
camera = cam.Camera.make_pinhole_camera()
camera_loc = np.array([0, 2, 0.5]).reshape(3,1)
camera_ori = np.array([np.pi/2, 0, 0]).reshape(3,1)
cam_loc = util.Pose(camera_loc, camera_ori).extrinsics()
ray0 = camera.ray_from_pixel((0,0), cam_loc)
ray1 = camera.ray_from_pixel((camera.width()-1,0), cam_loc)
ray2 = camera.ray_from_pixel((0, camera.height()-1), cam_loc)
ray3 = camera.ray_from_pixel((camera.width()-1, camera.height()-1), cam_loc)
ray4 = camera.ray_from_pixel((camera.width()/2,0), cam_loc)
ray5 = camera.ray_from_pixel((0, camera.height()/2), cam_loc)
ray6 = camera.ray_from_pixel((camera.width()-1, camera.height()/2), cam_loc)
ray7 = camera.ray_from_pixel((camera.width()/2, camera.height()-1), cam_loc)
rays = [ray0, ray1, ray2, ray3, ray4, ray5, ray6, ray7]
vis.plot_camera_with_rays(cam_loc, rays)
def line_motion():
poses = []
imu_sample_rate = 1000 #100Hz
time_length = 5
start_a = -np.pi / 3
end_a = np.pi / 3
theta = np.linspace(start_a, end_a, imu_sample_rate * time_length)
dtheta_dt = (end_a - start_a) / time_length
sine = np.sin(theta)
cosine = np.cos(theta)
timestamp = [
int(1.46065 * (10**18) + (10**9) / imu_sample_rate * t)
for t in range(imu_sample_rate * time_length)
]
for i in xrange(len(theta)):
loc = np.array([0, 0, -0.5]).reshape(3, 1)
ori = np.array([[cosine[i], 0, sine[i]], [0, 1, 0],
[-sine[i], 0, cosine[i]]])
lvel = np.zeros((3, 1))
#angular velocity
avel = np.array([0, dtheta_dt, 0])
lacc = np.zeros((3, 1))
aacc = np.zeros((3, 1))
pose = util.Pose(loc,
ori,
timestamp[i],
lvel=lvel,
avel=avel,
lacc=lacc,
aacc=aacc)
poses.append(pose)
return poses
def calib_with_covarage():
# parameters
exp_repeat_times = 50
noise3d_lvls = [0]
noise2d_lvls = [0.3]
board_height = [5]
board_width = [8]
board_sqsize = [0.23]
depth_min = 6 #m
depth_max = 9 #m
n = 50
coverage = [1.0, 0.5, 0.1]
for noise3d in noise3d_lvls:
for noise2d in noise2d_lvls:
true_cam = cam.Camera.nikon_camera()
# true_cam.intrinsics.radial_dist = 1*true_cam.intrinsics.radial_dist
# true_cam.intrinsics.tang_dist = 1*true_cam.intrinsics.tang_dist
cam_loc = np.zeros((3, 1))
cam_ori = np.zeros((3, 1))
cam_extrin = util.Pose(cam_loc, cam_ori).extrinsics()
bh = board_height[0]
bw = board_width[0]
bs = board_sqsize[0]
for cov in coverage:
estimations = []
for iexp in xrange(exp_repeat_times):
# Generate n boards
board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), noise3d)
perfect_board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), 0)
obs_list = []
# for i in xrange(n):
while len(obs_list) < n:
# choose a random pixel
mid_x = true_cam.width() / 2
mid_y = true_cam.height() / 2
pxl_x = np.random.random_integers(
int(mid_x - cov * mid_x), int(mid_x + cov * mid_x))
pxl_y = np.random.random_integers(
int(mid_y - cov * mid_y), int(mid_y + cov * mid_y))
# choose a random depth on ray from pixel
depth = np.random.rand() * (depth_max -
depth_min) + depth_min
# pt3d, ray_vec = true_cam.ray_from_pixel((pxl_x, pxl_y), cam_extrin)
# bd_loc = pt3d + depth*ray_vec
# choose a random orientation
bd_ori = util.random_rotation()
# board.move_board(bd_loc, bd_ori)
m_board = board.move_board_in_camera(
true_cam, cam_extrin, (pxl_x, pxl_y), depth,
bd_ori)
if m_board is not None:
print 'board at (', pxl_x, ',', pxl_y, ',', depth, '), rotation', bd_ori.flatten(
)
obs_list.append(
m_board.get_points()) #3xN np array
img_pts = true_cam.capture_images(cam_extrin, obs_list,
noise2d)
esti_cam = cam.Camera.calibrate_camera(img_pts,
perfect_board,
true_cam.size,
noDistortion=False)
# esti_cam = cam.Camera.calibrate_camera(img_pts, perfect_board, true_cam.size, noDistortion=True)
# esti_cam.intrinsics.radial_dist = np.zeros((3,))
# esti_cam.intrinsics.tang_dist = np.zeros((2,))
# vis.plot_camera_with_points(cam_extrin, obs_list)
# vis.plot_all_chessboards_in_camera(img_pts, true_cam.size, seperate_plot=True, save_name='results/capture_cov_'+str(cov)+'_'+str(iexp)+'.pdf')
estimations.append(esti_cam)
# Analyze error
vis.write_esti_results(estimations, true_cam, \
save_name_pre='results/coverage_'+str(cov)+'_board_'+str(n)+'_2dn_'+str(noise2d))
print "random {0} board experiment DONE".format(n)
import exp_util as util
import board as bd
import vis
import numpy as np
import cv2
import math
import matplotlib.pyplot as plt
from cycler import cycler
"""
IMU
"""
headset_motion = imu.read_motion('data/pose.csv', sample_ratio=1)
imu_rel_loc = np.array([0, 0, 0]).reshape(3, 1)
imu_rel_ori = np.array([0, math.pi, 0]).reshape(3, 1)
imu_rel_pose = util.Pose(imu_rel_loc, imu_rel_ori, time=0)
subsample_ratio = 10
imu_motion = imu.transform_motion(headset_motion,
imu_rel_pose,
subsample_ratio,
transform_deriviatives=True)
gravity_in_world = np.array([0, 9.81, 0])
imu.get_imu_readings(imu_motion,
gravity_in_world,
save_name='results/imu0.csv')
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# cm = plt.get_cmap('Blues')
# for i, p in enumerate(imu_motion):
noise3d = 0 noise2d = 0.5 bh = 50 bw = 80 bs = 0.023 depth_min = 0.5 #m depth_max = 5#m true_cam = cam.Camera.make_pinhole_camera() true_cam.intrinsics.radial_dist = 0*true_cam.intrinsics.radial_dist true_cam.intrinsics.tang_dist = 0*true_cam.intrinsics.tang_dist cam_loc = np.zeros((3,1)) cam_ori = np.zeros((3,1)) cam_extrin = util.Pose(cam_loc, cam_ori).extrinsics() # Generate n boards board = bd.Board.gen_calib_board((bw, bh), bs, \ np.zeros((3,1)), np.zeros((3,1)), noise3d) perfect_board = bd.Board.gen_calib_board((bw, bh), bs, \ np.zeros((3,1)), np.zeros((3,1)), 0) obs_list = [] # while len(obs_list) < n: # FIRST BOARD # choose a random pixel pxl_x0 = 1254 pxl_y0 = 505 depth0 = 3.76316885531 # choose a random orientation bd_ori0 = np.array([-1.4262966, 3.22082211, 2.47569224])
def pose(self):
return util.Pose(self.get_inv_location(), self.rot_mat.T,
self.time_stamp)
ax = fig.add_subplot(111, projection='3d')
cm = plt.get_cmap('Blues')
#for i, p in enumerate(imu_motion):
# ax = p.plot(ax, clr='b')
# ax.set_aspect('equal')
gravity_in_world = np.array([0, 9.81, 0])
imu.get_imu_readings(imu_motion,
gravity_in_world,
save_name='results/imu0.csv')
"""
Camera
"""
rel_loc = np.array([0, 0, 0]).reshape(3, 1)
rel_ori = np.array([0, 0, 0]).reshape(3, 1)
rel_pose = util.Pose(rel_loc, rel_ori, time=0)
cam_sampling_ratio = 20 # camera samples once when imu samples 5 times
cam_motion = imu.transform_motion(imu_motion, rel_pose, cam_sampling_ratio)
cm = plt.get_cmap('Oranges')
#for i, p in enumerate(cam_motion):
# ax = p.plot(ax, clr='r')
camera = cam.Camera.make_pinhole_camera()
camera.intrinsics.radial_dist = np.zeros((1, 3))
camera.intrinsics.tang_dist = np.zeros((1, 2))
"""
Board
"""
board_dim = (2, 2)
board_loc = np.array([-0.3564, 0.3564, 0]).reshape(3, 1)
board_ori = np.array([math.pi, 0, 0]).reshape(3, 1)
def circle_motion():
"""
A hard coded motion.
Returns a list of time-stamped poses.
"""
poses = []
imu_sample_rate = 100 #100Hz
time_length = 30
start_a = -16 * np.pi
end_a = 16 * np.pi
#padding = np.pi / 8
theta = np.linspace(start_a, end_a, imu_sample_rate * (time_length))
#theta = np.linspace(start_a, end_a, imu_sample_rate*(time_length-2))
slope = (end_a - start_a) / (time_length)
print 'speed', slope
#front = np.linspace(start_a-padding, start_a, imu_sample_rate)
#front = front*front*slope/2
#tend = np.linspace(end_a, end_a+padding, imu_sample_rate)
#tend = (tend-time_length)*(tend-time_length)*(-slope/2)
#theta = np.concatenate((front, theta, tend))
timestamp = [
int(1.46065 * (10**18) + (10**9) / imu_sample_rate * t)
for t in range(len(theta))
]
r = 1
zstart = 2
zend = 0.5
x = r * np.cos(theta)
y = r * np.sin(theta)
# z = -2*r * np.ones(theta.shape)
z = np.linspace(zstart, zend, imu_sample_rate * time_length)
offset = 0.3564
dz_dt = (zend - zstart) / time_length
ra = math.pi / 6
dtheta_dt = (end_a - start_a) / (time_length) * np.ones(
(time_length * imu_sample_rate, ))
#dtheta_dt = (end_a-start_a)/(time_length-2) * np.ones(((time_length-2)*imu_sample_rate,))
#dtheta_front = slope * np.linspace(0, 1, imu_sample_rate)
#dtheta_end = slope * np.linspace(1, 0, imu_sample_rate)
#dtheta_dt = np.concatenate((dtheta_front,dtheta_dt,dtheta_end))
ddtheta_dt = np.zeros((imu_sample_rate * (time_length), ))
#ddtheta_dt = np.zeros((imu_sample_rate*(time_length-2), ))
#ddtheta_front = slope * np.ones((imu_sample_rate,))
#ddtheta_end = -slope * np.ones((imu_sample_rate,))
#ddtheta_dt = np.concatenate((ddtheta_front, ddtheta_dt, ddtheta_end))
for i in xrange(len(theta)):
loc = np.array([x[i] + offset, y[i] + offset, z[i]]).reshape(3, 1)
ori = np.array([[np.cos(ra)*np.cos(theta[i]), np.cos(ra)*np.sin(theta[i]), -np.sin(ra)], \
[np.sin(theta[i]), -np.cos(theta[i]), 0], \
[-np.sin(ra)*np.cos(theta[i]), -np.sin(ra)*np.sin(theta[i]), -np.cos(ra)]]).T
# lvel = np.array( [-y[i], x[i], 0] ).reshape(3,1)
lvel = np.array([-y[i] * dtheta_dt[i], x[i] * dtheta_dt[i],
dz_dt]).reshape(3, 1)
#angular velocity
avel = np.array([0, 0, dtheta_dt[i]])
lacc = np.array([-x[i], -y[i], 0]).reshape(3, 1) * (dtheta_dt[i]**2
) #TODO
aacc = np.array([0, 0, 0]).reshape(3, 1)
pose = util.Pose(loc,
ori,
timestamp[i],
lvel=lvel,
avel=avel,
lacc=lacc,
aacc=aacc)
poses.append(pose)
return poses
def calib_with_random_n_boards(n):
"""
Generate n boards at random angle and depth, experiment for calibration result
under different 3d noise, 2d noise, number of control points;
distortion, focal length, and center point.
"""
# parameters
exp_repeat_times = 50
noise3d_lvls = [0]
# noise3d_lvls = [0, 0.005, 0.01, 0.04]
noise2d_lvls = [0]
board_height = [5]
board_width = [8]
board_sqsize = [0.23]
depth_min = 0.5 #m
depth_max = 5 #m
for noise3d in noise3d_lvls:
for noise2d in noise2d_lvls:
# @TODO: experiment with different camera parameters?
true_cam = cam.Camera.make_pinhole_camera()
cam_loc = np.zeros((3, 1))
cam_ori = np.zeros((3, 1))
cam_extrin = util.Pose(cam_loc, cam_ori).extrinsics()
for bh in board_height:
for bw in board_width:
for bs in board_sqsize:
estimations = []
for iexp in xrange(exp_repeat_times):
# Generate n boards
board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), noise3d)
perfect_board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), 0)
obs_list = []
# for i in xrange(n):
while len(obs_list) < n:
# choose a random pixel
pxl_x = np.random.random_integers(
0,
true_cam.width() - 1)
pxl_y = np.random.random_integers(
0,
true_cam.height() - 1)
# choose a random depth on ray from pixel
depth = np.random.rand() * (
depth_max - depth_min) + depth_min
# pt3d, ray_vec = true_cam.ray_from_pixel((pxl_x, pxl_y), cam_extrin)
# bd_loc = pt3d + depth*ray_vec
# choose a random orientation
bd_ori = util.random_rotation()
# board.move_board(bd_loc, bd_ori)
m_board = board.move_board_in_camera(
true_cam, cam_extrin, (pxl_x, pxl_y),
depth, bd_ori)
if m_board is not None:
obs_list.append(
m_board.get_points()) #3xN np array
img_pts = true_cam.capture_images(
cam_extrin, obs_list, noise2d)
esti_cam = cam.Camera.calibrate_camera(
img_pts, perfect_board, true_cam.size)
# vis.plot_camera_with_points(cam_extrin, obs_list)
vis.plot_all_chessboards_in_camera(
img_pts,
true_cam.size,
seperate_plot=True,
save_name='results/capture_' + str(n) +
'_3dn_' + str(noise3d) + '_2dn_' +
str(noise2d) + '_bn_' + str(bh * bw) + '_bs_' +
str(bs) + '_' + str(iexp) + '.pdf')
estimations.append(esti_cam)
# Analyze error
vis.write_esti_results(estimations, true_cam, \
save_name_pre='results/report_'+str(n)+'_3dn_'+str(noise3d)+'_2dn_'+str(noise2d)+'_bn_'+str(bh*bw)+'_bs_'+str(bs))
print "random {0} board experiment DONE".format(n)
def circle_motion():
"""
A hard coded motion.
Returns a list of time-stamped poses.
"""
poses = []
imu_sample_rate = 100 #100Hz
time_length = 30
start_a = -8 * np.pi
end_a = 8 * np.pi
padding = np.pi / 8
theta = np.linspace(start_a, end_a, imu_sample_rate * (time_length - 2))
front = np.linspace(start_a - padding, start_a, imu_sample_rate)
tend = np.linspace(end_a, end_a + padding, imu_sample_rate)
theta = np.concatenate((front, theta, tend))
timestamp = [
int(1.46065 * (10**18) + (10**9) / imu_sample_rate * t)
for t in range(len(theta))
]
r = 1
x = r * np.cos(theta)
y = r * np.sin(theta)
# z = -2*r * np.ones(theta.shape)
z = np.linspace(-2, -1, imu_sample_rate * time_length)
ra = math.pi / 6
dtheta_dt = (end_a - start_a) / (time_length - 2) * np.ones(
((time_length - 2) * imu_sample_rate, ))
dtheta_dt = np.concatenate((padding * np.ones(
(imu_sample_rate, )), dtheta_dt, padding * np.ones(
(imu_sample_rate, ))))
for i in xrange(len(theta)):
loc = np.array([x[i], y[i], z[i]]).reshape(3, 1)
ori = np.array([[np.cos(ra)*np.cos(theta[i]), np.cos(ra)*np.sin(theta[i]), np.sin(ra)], \
[-np.sin(theta[i]), np.cos(theta[i]), 0], \
[-np.sin(ra)*np.cos(theta[i]), -np.sin(ra)*np.sin(theta[i]), np.cos(ra)]]).T
# lvel = np.array( [-y[i], x[i], 0] ).reshape(3,1)
lvel = np.array([-y[i], x[i], dtheta_dt[i]]).reshape(3, 1)
#angular velocity
avel = np.array([0, 0, dtheta_dt[i]])
lacc = np.array([-x[i], -y[i], 0]).reshape(3, 1)
aacc = np.array([0, 0, 0]).reshape(3, 1)
pose = util.Pose(loc,
ori,
timestamp[i],
lvel=lvel,
avel=avel,
lacc=lacc,
aacc=aacc)
poses.append(pose)
# import pdb;pdb.set_trace()
# ra = math.pi/6
# rx = -ra * np.sin(theta)
# ry = -ra * np.cos(theta)
# rz = np.zeros(theta.shape)
# for i in xrange(len(theta)):
# loc = np.array( [x[i], y[i], z[i]] ).reshape(3,1)
# ori = np.array( [rx[i], ry[i], rz[i]] ).reshape(3,1)
# lvel = np.array( [-y[i], x[i], 0] ).reshape(3,1)
# avel = np.array( [ry[i], -rx[i], 0] ).reshape(3,1)
# lacc = np.array( [-x[i], -y[i], 0] ).reshape(3,1)
# aacc = np.array( [-rx[i], -ry[i], 0]).reshape(3,1)
# pose = util.Pose(loc, ori, timestamp[i], lvel=lvel, avel=avel, lacc=lacc, aacc=aacc)
# poses.append(pose)
return poses
def calib_with_angle():
angle_change = [0, 0.01, 0.1, 1]
# parameters
exp_repeat_times = 100
noise3d_lvls = [0]
noise2d_lvls = [0.3]
board_height = [5]
board_width = [8]
board_sqsize = [0.23]
depth_min = 3 #m
depth_max = 5 #m
n = 3
for noise3d in noise3d_lvls:
for noise2d in noise2d_lvls:
true_cam = cam.Camera.nikon_camera()
# true_cam.intrinsics.radial_dist = 1*true_cam.intrinsics.radial_dist
# true_cam.intrinsics.tang_dist = 1*true_cam.intrinsics.tang_dist
cam_loc = np.zeros((3, 1))
cam_ori = np.zeros((3, 1))
cam_extrin = util.Pose(cam_loc, cam_ori).extrinsics()
bh = board_height[0]
bw = board_width[0]
bs = board_sqsize[0]
for ac in angle_change:
estimations = []
start_angle = util.random_rotation()
#check start_angle works
board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), noise3d)
m_board = board.move_board_in_camera(true_cam, cam_extrin, \
(true_cam.width()/2, true_cam.height()/2), 4, start_angle)
while m_board is None:
start_angle = util.random_rotation()
m_board = board.move_board_in_camera(true_cam, cam_extrin, \
(true_cam.width()/2, true_cam.height()/2), 4, start_angle)
start_angle = cv2.Rodrigues(start_angle)[0]
for iexp in xrange(exp_repeat_times):
# Generate n boards
perfect_board = bd.Board.gen_calib_board((bw, bh), bs, \
np.zeros((3,1)), np.zeros((3,1)), 0)
obs_list = []
# for i in xrange(n):
while len(obs_list) < n:
# choose a random pixel
pxl_x = np.random.random_integers(
0,
true_cam.width() - 1)
pxl_y = np.random.random_integers(
0,
true_cam.height() - 1)
# choose a random depth on ray from pixel
depth = np.random.rand() * (depth_max -
depth_min) + depth_min
# pt3d, ray_vec = true_cam.ray_from_pixel((pxl_x, pxl_y), cam_extrin)
# bd_loc = pt3d + depth*ray_vec
# choose a random orientation
bd_ori = start_angle.dot(
cv2.Rodrigues(util.random_rot_w_scale(ac))[0])
# board.move_board(bd_loc, bd_ori)
m_board = board.move_board_in_camera(
true_cam, cam_extrin, (pxl_x, pxl_y), depth,
bd_ori)
if m_board is not None:
# print 'board at (', pxl_x, ',', pxl_y,',', depth, '), rotation', bd_ori.flatten()
obs_list.append(
m_board.get_points()) #3xN np array
img_pts = true_cam.capture_images(cam_extrin, obs_list,
noise2d)
esti_cam = cam.Camera.calibrate_camera(img_pts,
perfect_board,
true_cam.size,
noDistortion=False)
# esti_cam = cam.Camera.calibrate_camera(img_pts, perfect_board, true_cam.size, noDistortion=True)
# esti_cam.intrinsics.radial_dist = np.zeros((3,))
# esti_cam.intrinsics.tang_dist = np.zeros((2,))
# vis.plot_camera_with_points(cam_extrin, obs_list)
# vis.plot_all_chessboards_in_camera(img_pts, true_cam.size, seperate_plot=True, save_name='results/capture_rot_'+str(ac)+'_'+str(iexp)+'.pdf')
estimations.append(esti_cam)
# Analyze error
vis.write_esti_results(estimations, true_cam, \
save_name_pre='results/rot_'+str(ac)+'_board_'+str(n)+'_2dn_'+str(noise2d))
print "depth {0} board experiment DONE".format(n)
