def calculate_ls(self,iters, ransac_iters):
self.ls_beta = pose_estimate.least_square(self.X, self.Y)
self.ls_pose = tf.get_projection_matrix(self.ls_beta)
self.ls_position = self.ls_pose.dot(self.ls_position)
self.me_beta = pose_estimate.m_estimator(self.X, self.Y, iters)
self.me_pose = tf.get_projection_matrix(self.me_beta)
self.m_position = self.me_pose.dot(self.m_position)
init_beta = pose_estimate.ransac_ls(self.X,self.Y,ransac_iters,3)
self.ransac_beta = pose_estimate.m_estimator(self.X, self.Y, iters, init_beta)
self.ransac_pose = tf.get_projection_matrix(self.ransac_beta)
self.r_position = self.ransac_pose.dot(self.r_position)
self.ls_noise_beta = pose_estimate.least_square(self.X_n, self.Y_n)
self.ls_noise_pose = tf.get_projection_matrix(self.ls_noise_beta)
self.ls_position_noise = self.ls_noise_pose.dot(self.ls_position_noise)
self.me_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters)
self.me_noise_pose = tf.get_projection_matrix(self.me_noise_beta)
self.m_position_noise = self.me_noise_pose.dot(self.m_position_noise)
# init_beta = pose_estimate.ransac_ls(self.X,self.Y,50,3)
# self.ransac_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters, init_beta)
# self.ransac_noise_pose = tf.get_projection_matrix(self.ransac_noise_beta)
# self.r_position_noise = self.ransac_noise_pose.dot(self.r_position_noise)
bp = tf.pose_to_beta(self.me_pose)
self.kf_pose_ls.predict()
self.kf_pose_ls.update(bp.T)
print('-----')
print(ut.to_string(bp))
print(ut.to_string(self.kf_pose_ls.x.T))
print('-----')
def move_points(self, tx, ty, tz, rx, ry, rz):
self.projection = tf.get_projection_matrix(
np.array([tx, ty, tz, rx, ry, rz]))
self.next_pts_3d = np.dot(self.projection, self.prev_pts_3d)
self.next_pts_2d = self.camera.dot(self.next_pts_3d)
self.next_pts_2d = self.next_pts_2d / self.next_pts_3d[2, :]
self.next_noise_2d = self.next_pts_2d.copy()
# print('prev')
# print(ut.to_string(self.prev_pts_2d))
# print('next')
# print(ut.to_string(self.next_pts_2d))
self.x_flow = self.next_pts_2d[0, :] - self.prev_pts_2d[0, :]
self.y_flow = self.next_pts_2d[1, :] - self.prev_pts_2d[1, :]
self.d_pts = self.next_pts_3d[2, :]
self.x_flow_noise = self.x_flow.copy()
self.y_flow_noise = self.y_flow.copy()
num_points = self.next_pts_3d.shape[1]
ids = np.arange(num_points)
np.random.shuffle(ids)
num_elems = int(num_points * self.noise_percentage)
elems = ids[0:num_elems]
x_noise = np.random.normal(self.noise_mu, self.noise_sigma, num_elems)
y_noise = np.random.normal(self.noise_mu, self.noise_sigma, num_elems)
for i in range(0, num_elems):
id = elems[i]
self.x_flow_noise[id] += x_noise[i]
self.y_flow_noise[id] += y_noise[i]
self.next_noise_2d[0, id] += x_noise[i]
self.next_noise_2d[1, id] += y_noise[i]
#[X,Y] = get_lq_data(self.prev_pts_2d, self.next_pts_2d, self.d_pts, self.camera)
[X, Y] = pose_estimate.get_normal_equations(self.prev_pts_2d.T,
self.next_pts_2d.T,
self.d_pts, self.camera)
[self.X_n, self.Y_n
] = pose_estimate.get_normal_equations(self.prev_noise_2d.T,
self.next_noise_2d.T,
self.d_pts, self.camera)
self.X = X
self.Y = Y
self.calculate_ls(10, 10)
self.calculate_distance()
self.prev_pts_2d = self.next_pts_2d.copy()
self.prev_pts_3d = self.next_pts_3d.copy()
self.prev_noise_2d = self.next_noise_2d.copy()
self.prev_noise_3d = self.next_pts_3d.copy()
def get_fixed_data(self, num_pts):
cam = np.array([[self.fx, 0, self.cx, 0], [0, self.fy, self.cy, 0],
[0, 0, 1, 0]])
self.camera = cam
camera_inv = np.linalg.inv(cam[0:3, 0:3])
[self.init_pts_2d,
pts_d] = create_fixed_points(num_pts, self.cx, self.cy)
num_points = self.init_pts_2d.shape[1]
self.init_pts_3d = np.dot(camera_inv, self.init_pts_2d[0:3, :]) * pts_d
self.init_pts_3d = np.vstack([self.init_pts_3d, np.ones(num_points)])
self.ls_position = tf.get_projection_matrix(
np.array([0, 0, 0, 0, 0, 0]))
self.ls_position_noise = tf.get_projection_matrix(
np.array([0, 0, 0, 0, 0, 0]))
self.m_position = tf.get_projection_matrix(np.array([0, 0, 0, 0, 0,
0]))
self.m_position_noise = tf.get_projection_matrix(
np.array([0, 0, 0, 0, 0, 0]))
self.r_position = tf.get_projection_matrix(np.array([0, 0, 0, 0, 0,
0]))
self.r_position_noise = tf.get_projection_matrix(
np.array([0, 0, 0, 0, 0, 0]))
self.prev_pts_2d = self.init_pts_2d.copy()
self.prev_pts_3d = self.init_pts_3d.copy()
self.prev_noise_2d = self.init_pts_2d.copy()
self.prev_noise_3d = self.init_pts_3d.copy()
def move_points(self,tx,ty,tz,rx,ry,rz):
self.projection = tf.get_projection_matrix(np.array([tx,ty,tz,rx,ry,rz]))
self.next_pts_3d = np.dot(self.projection, self.prev_pts_3d)
self.next_pts_2d = self.camera.dot(self.next_pts_3d)
self.next_pts_2d = self.next_pts_2d / self.next_pts_3d[2,:]
self.next_noise_2d = self.next_pts_2d.copy()
# print('prev')
# print(ut.to_string(self.prev_pts_2d))
# print('next')
# print(ut.to_string(self.next_pts_2d))
self.x_flow = self.next_pts_2d[0,:] - self.prev_pts_2d[0,:]
self.y_flow = self.next_pts_2d[1,:] - self.prev_pts_2d[1,:]
self.d_pts = self.next_pts_3d[2,:]
self.x_flow_noise = self.x_flow.copy()
self.y_flow_noise = self.y_flow.copy()
num_points = self.next_pts_3d.shape[1]
ids = np.arange(num_points)
np.random.shuffle(ids)
num_elems = int(num_points * self.noise_percentage)
elems = ids[0:num_elems]
x_noise = np.random.normal(self.noise_mu, self.noise_sigma, num_elems)
y_noise = np.random.normal(self.noise_mu, self.noise_sigma, num_elems)
for i in range(0,num_elems):
id = elems[i]
self.x_flow_noise[id] += x_noise[i]
self.y_flow_noise[id] += y_noise[i]
self.next_noise_2d[0,id] += x_noise[i]
self.next_noise_2d[1,id] += y_noise[i]
#[X,Y] = get_lq_data(self.prev_pts_2d, self.next_pts_2d, self.d_pts, self.camera)
[X,Y] = pose_estimate.get_normal_equations(self.prev_pts_2d.T, self.next_pts_2d.T, self.d_pts, self.camera)
[self.X_n, self.Y_n] = pose_estimate.get_normal_equations(self.prev_noise_2d.T, self.next_noise_2d.T, self.d_pts, self.camera)
self.X = X
self.Y = Y
self.calculate_ls(10, 10)
self.calculate_distance()
self.prev_pts_2d = self.next_pts_2d.copy()
self.prev_pts_3d = self.next_pts_3d.copy()
self.prev_noise_2d = self.next_noise_2d.copy()
self.prev_noise_3d = self.next_pts_3d.copy()
def print_poses(self):
print('gt pose\n' + ut.to_string(self.projection))
#print('ls pose\n' + ut.to_string(self.ls_pose))
print('m pose\n' + ut.to_string(self.me_pose))
#fprint('r pose\n' + ut.to_string(self.ransac_pose))
k_pose = self.kf_pose_ls.x
tmp = np.array([k_pose[0,0],k_pose[1,0],k_pose[2,0],k_pose[9,0],k_pose[10,0],k_pose[11,0]])
k_pose = tf.get_projection_matrix(tmp)
print(ut.to_string(self.kf_pose_ls.x.T))
print('k pose\n' + ut.to_string(k_pose))
def calculate_ls(self, iters, ransac_iters):
self.ls_beta = pose_estimate.least_square(self.X, self.Y)
self.ls_pose = tf.get_projection_matrix(self.ls_beta)
self.ls_position = self.ls_pose.dot(self.ls_position)
self.me_beta = pose_estimate.m_estimator(self.X, self.Y, iters)
self.me_pose = tf.get_projection_matrix(self.me_beta)
self.m_position = self.me_pose.dot(self.m_position)
init_beta = pose_estimate.ransac_ls(self.X, self.Y, ransac_iters, 3)
self.ransac_beta = pose_estimate.m_estimator(self.X, self.Y, iters,
init_beta)
self.ransac_pose = tf.get_projection_matrix(self.ransac_beta)
self.r_position = self.ransac_pose.dot(self.r_position)
self.ls_noise_beta = pose_estimate.least_square(self.X_n, self.Y_n)
self.ls_noise_pose = tf.get_projection_matrix(self.ls_noise_beta)
self.ls_position_noise = self.ls_noise_pose.dot(self.ls_position_noise)
self.me_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n,
iters)
self.me_noise_pose = tf.get_projection_matrix(self.me_noise_beta)
self.m_position_noise = self.me_noise_pose.dot(self.m_position_noise)
# init_beta = pose_estimate.ransac_ls(self.X,self.Y,50,3)
# self.ransac_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters, init_beta)
# self.ransac_noise_pose = tf.get_projection_matrix(self.ransac_noise_beta)
# self.r_position_noise = self.ransac_noise_pose.dot(self.r_position_noise)
bp = tf.pose_to_beta(self.me_pose)
self.kf_pose_ls.predict()
self.kf_pose_ls.update(bp.T)
print('-----')
print(ut.to_string(bp))
print(ut.to_string(self.kf_pose_ls.x.T))
print('-----')
def print_poses(self):
print('gt pose\n' + ut.to_string(self.projection))
#print('ls pose\n' + ut.to_string(self.ls_pose))
print('m pose\n' + ut.to_string(self.me_pose))
#fprint('r pose\n' + ut.to_string(self.ransac_pose))
k_pose = self.kf_pose_ls.x
tmp = np.array([
k_pose[0, 0], k_pose[1, 0], k_pose[2, 0], k_pose[9, 0],
k_pose[10, 0], k_pose[11, 0]
])
k_pose = tf.get_projection_matrix(tmp)
print(ut.to_string(self.kf_pose_ls.x.T))
print('k pose\n' + ut.to_string(k_pose))
def gen_data(self, num_points):
cam = np.array([[self.fx, 0, self.cx, 0], [0, self.fy, self.cy, 0], [0, 0, 1, 0]])
self.camera = cam
camera_inv = np.linalg.inv(cam[0:3,0:3])
[self.init_pts_2d, pts_d] = create_image_points(num_points, 640, 480)
self.init_pts_3d = np.dot(camera_inv, self.init_pts_2d[0:3,:]) * pts_d
self.init_pts_3d = np.vstack([self.init_pts_3d, np.ones(num_points)])
self.ls_position = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.ls_position_noise = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.m_position = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.m_position_noise = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.r_position = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.r_position_noise = tf.get_projection_matrix(np.array([0,0,0,0,0,0]))
self.prev_pts_2d = self.init_pts_2d.copy()
self.prev_pts_3d = self.init_pts_3d.copy()
self.prev_noise_2d = self.init_pts_2d.copy()
self.prev_noise_3d = self.init_pts_3d.copy()
