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()