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
