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 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 test_filter(filter): for i in range(0,5): obs = np.array([[0.1*i,0.05*i,0.02*i,0.01*i,0.005*i,0.002*i]]).T filter.predict() filter.update(obs) print(ut.to_string(obs.T,2)) res = np.array([filter.x[0,0],filter.x[1,0],filter.x[2,0],filter.x[9,0],filter.x[10,0],filter.x[11,0]]) print(ut.to_string(filter.x.T,2)) print(ut.to_string(res.T,2)) return filter
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 main(argv=None): if argv is None: argv = sys.argv[1:] p = ArgumentParser(prog='module') #p.add_argument('shell', choices=('bash',)) p.add_argument('--debug', action='store_true', default=False) p.add_argument('-v', default=MODULEVERBOSITY, type=int, help='Level of verbosity [default: %(default)s]') p.add_argument('-c', action='store_true', default=False, help=('Call the c modulecmd (must define MODULECMD ' 'environment variable)')) sub_p = p.add_subparsers(dest='subparser_name', title='subcommands', description='valid subcommands', help='sub-command help') p_avail = sub_p.add_parser('avail', help='Display available modules') p_avail.add_argument( '-t', default=False, action='store_true', help='Display available modules in terse format [default: False]') p_list = sub_p.add_parser('list', help='Display loaded modules') p_list.add_argument( '-t', default=False, action='store_true', help='Display available modules in terse format [default: False]') p_load = sub_p.add_parser('load', help='Load module[s]') p_load.add_argument('modulefile', nargs='+', help='Valid modulefile[s]') p_load.add_argument( '-f', '--force', action='store_true', default=False, help='Force load missing prerequisites [default: False]') p_uload = sub_p.add_parser('unload', help='Unload module[s]') p_uload.add_argument('modulefile', nargs='+', help='Valid modulefile[s]') p_uload.add_argument('-f', '--force', action='store_true', default=False, help='Remove loaded prerequisites [default: False]') p_purge = sub_p.add_parser('purge', help='Unload all modules') p_swap = sub_p.add_parser('swap', help='Swap modules') p_swap.add_argument('modulefile', nargs=2, help='Valid modulefile') p_help = sub_p.add_parser('help', help='Display help for module[s]') p_help.add_argument('modulefile', nargs='+', help='Valid modulefile[s]') p_what = sub_p.add_parser('whatis', help='Display short help for module[s]') p_what.add_argument('modulefile', nargs='*', help='Valid modulefile[s]') args = p.parse_args(argv) if args.debug: ut.global_opts.debug = True try: ut.global_opts.force = args.force except AttributeError: pass #p = PyModuleCommand(args.shell, v=args.v) p = PyModuleCommand(v=args.v) if args.c: try: cargs = args.modulefile except AttributeError: cargs = [] height, width = ut.get_console_dims() space = ' ' * 12 text = wrap( '***Warning: modules loaded/unloaded by calling TCL ' 'modules cannot be tracked with this modules package ' 'and is not recommended', width, subsequent_indent=space) ut.put_stderr('\n'.join(text)) kwargs = {'sh_out': 1} p = ut.call_tcl_modulecmd(shell.name, args.subparser_name, *cargs, **kwargs) if p['returncode'] != 0: ut.raise_error(p['stderr']) if args.subparser_name in (AVAIL, LIST, HELP, WHATIS): ut.put_stderr(ut.to_string(p['stderr'])) elif bool(p['stderr']): error(p['stderr']) else: ut.put_stderr(ut.to_string(p['stdout'])) elif args.subparser_name == 'avail': p.display_available_modules(terse=args.t) elif args.subparser_name == 'list': p.list_loaded_modules(terse=args.t) elif args.subparser_name == 'load': p.load_modules_from_cl(*args.modulefile) elif args.subparser_name == 'unload': p.unload_modules_from_cl(*args.modulefile) elif args.subparser_name == 'swap': p.swap_modules_from_cl(*args.modulefile) elif args.subparser_name == 'help': p.help1(*args.modulefile) elif args.subparser_name == 'whatis': p.whatis1(*args.modulefile) elif args.subparser_name == 'purge': p.purge_from_cl() return 0