img.augment(img.ref_obj), img.ref_img)
#--- Individual Robot position ---#
img.r_obj, err2, err3 = calib.get_leastsquares(
[cam], [img.augment(img.r_img)], 'hz', R_HEIGHT, r_real)
imgs[i] = img
cams[i] = cam
pts[i] = img.augment(img.r_img)
pts_ref[i] = img.augment(img.ref_img)
errs[i] = err_img
p_lq_fix, p_lq_free = leastsquares(choice_ref, pts_ref, cams, errs,
img, pts, r_real, loop_counter,
TIME)
name = 'posobj_fix_' + str(choice_ref) + '_' + TIME + '.txt'
p_lq_fix_wall = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
p_lq_fix.T[0])
calib.write_pos(out_dir, name, p_lq_fix_wall[0])
name = 'posobj_free_' + str(choice_ref) + '_' + TIME + '.txt'
p_lq_free_wall = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
p_lq_free.T)
calib.write_pos(out_dir, name, p_lq_free_wall[0])
#--------------------------- 4.2 Odometry Localization ------------------#
if DEBUG:
choice = 'n'
else:
choice = input(ROBOT_ODO)
if choice == 'y':
# Connect to robot
if not robot_connected:
def plot_geometry(self, fname='', zoom=False):
'''
Plots all results read by read_files() in the wall reference frame.
Run read_files() before running this function.
_Parameters_:
[fname:] file name where resulting image is saved (within self.out_dir)
no output saved if not specified or set to '' (default '')
[zoom:] weather to zoom to positions or not. (default False)
_Returns_: Nothing
'''
if zoom:
plt.figure(figsize=(10, 5))
else:
plt.figure(figsize=(9, 8))
legend_str = []
if not zoom:
# plot line between frist two reference points
ax = plt.plot(PTS_BASIS[:, 0],
PTS_BASIS[:, 1],
color='k',
linestyle='-')
legend_str.append('ref basis')
# plot reference coordinate system
origin = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
np.matrix([0, 0, 0]))
xaxis = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
np.matrix([MARGIN[1], 0, 0]))
yaxis = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
np.matrix([0, MARGIN[1], 0]))
plt.plot([yaxis[0, 0], origin[0, 0], xaxis[0, 0]],
[yaxis[0, 1], origin[0, 1], xaxis[0, 1]],
color='b',
linestyle=':')
legend_str.append('ref origin')
# plot wall
x = [0, WIDTH_ROOM, WIDTH_ROOM, 0, 0]
y = [0, 0, HEIGHT_ROOM, HEIGHT_ROOM, 0]
plt.plot(x, y, linestyle='dashed', color='k')
legend_str.append('walls')
# plot robot positions
if self.output_real != '':
plt.plot(self.real[:, 0], self.real[:, 1], marker='o', color='g')
legend_str.append('real')
# plot visual positions
if self.output_vis_fix != '':
if len(self.vis_fix.shape) > 1:
plt.plot(self.vis_fix[:, 0],
self.vis_fix[:, 1],
marker='x',
color='r')
else:
plt.plot(self.vis_fix[0],
self.vis_fix[1],
marker='x',
color='r')
if zoom:
error = np.linalg.norm(self.real[:, :2] - self.vis_fix[:, :2]
) / self.real.shape[0]
legend_str.append(
'visual fixed RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('visual fixed')
if self.output_vis_free != '':
if len(self.vis_free.shape) > 1:
plt.plot(self.vis_free[:, 0],
self.vis_free[:, 1],
marker='x',
color='grey')
else:
plt.plot(self.vis_free[0],
self.vis_free[1],
marker='x',
color='grey')
if zoom:
error = np.linalg.norm(self.real[:, :2] - self.vis_free[:, :2]
) / self.real.shape[0]
legend_str.append(
'visual free RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('visual free')
# plot odometry positions
if self.output_enc != '':
pos_0 = [round(self.real[0, 0]), round(self.real[0, 1]), THETA_0]
self.odo = self.odometry(pos_0)
plt.plot(self.odo[:, 0], self.odo[:, 1], marker='x', color='k')
if zoom:
error = np.linalg.norm(self.real[:, :2] -
self.odo[:, :2]) / self.real.shape[0]
legend_str.append(
'odometry RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('odometry')
# plot expected robot movement
if self.output_mov != '':
plt.plot(self.mov[:, 0],
self.mov[:, 1],
marker='o',
markersize=2,
color='k')
error = np.linalg.norm(self.real[:, :2] -
self.mov[:, :2]) / self.real.shape[0]
legend_str.append('expected RMS-error = {0:3.2f}'.format(error))
# plot camera positions
if self.output_cam != '' and not zoom:
cam_wall = []
# first time: change to wall reference frame.
if self.camwall == '':
for c in self.cam:
res = calib.change_ref_to_wall(PTS_BASIS, MARGIN,
c[1:].reshape(1, 3))
c[1:] = res[0]
cam_wall.append(c)
self.camwall = np.array(cam_wall)
colors = {139: 'blue', 141: 'red', 143: 'green', 145: 'orange'}
for i in range(0, self.camwall.shape[0]):
plt.plot(self.camwall[i, 1],
self.camwall[i, 2],
marker='x',
linestyle='',
color=colors[self.camwall[i, 0]])
#legend_str.append(str(int(self.camwall[i,0])))
if self.output_camreal != '' and not zoom:
colors = {139: 'blue', 141: 'red', 143: 'green', 145: 'orange'}
for i in range(0, self.camreal.shape[0]):
plt.plot(self.camreal[i, 1],
self.camreal[i, 2],
marker='o',
linestyle='',
color=colors[self.camreal[i, 0]])
#legend_str.append(str(int(self.camreal[i,0])))
plt.legend(legend_str, 'best')
# plot properties
plt.grid(True)
plt.xlabel('x [mm]'), plt.ylabel('y [mm]')
if zoom:
#plt.xlim([2000,5000])
#plt.ylim([2000,5000])
ax = plt.gca()
ax.relim()
ax.autoscale_view()
plt.axes().set_aspect('equal')
plt.title('Experimental Results - Robot Positions\n')
else:
plt.axes().set_aspect('equal')
plt.xlim([-1000, 8000])
plt.ylim([-1000, 8000])
plt.title('Experimental Results in Room Reference Frame \n')
plt.show(block=False)
if fname != '':
plt.savefig(self.out_dir + fname)
def plot_geometry(self,fname='',zoom=False):
'''
Plots all results read by read_files() in the wall reference frame.
Run read_files() before running this function.
_Parameters_:
[fname:] file name where resulting image is saved (within self.out_dir)
no output saved if not specified or set to '' (default '')
[zoom:] weather to zoom to positions or not. (default False)
_Returns_: Nothing
'''
if zoom:
plt.figure(figsize=(10,5))
else:
plt.figure(figsize=(9,8))
legend_str=[]
if not zoom:
# plot line between frist two reference points
ax = plt.plot(PTS_BASIS[:,0],PTS_BASIS[:,1],color='k',linestyle='-')
legend_str.append('ref basis')
# plot reference coordinate system
origin=calib.change_ref_to_wall(PTS_BASIS,MARGIN,np.matrix([0,0,0]))
xaxis=calib.change_ref_to_wall(PTS_BASIS,MARGIN,np.matrix([MARGIN[1],0,0]))
yaxis=calib.change_ref_to_wall(PTS_BASIS,MARGIN,np.matrix([0,MARGIN[1],0]))
plt.plot([yaxis[0,0],origin[0,0],xaxis[0,0]],[yaxis[0,1],origin[0,1],xaxis[0,1]],color='b',linestyle=':')
legend_str.append('ref origin')
# plot wall
x=[0,WIDTH_ROOM,WIDTH_ROOM,0,0]
y=[0,0,HEIGHT_ROOM,HEIGHT_ROOM,0]
plt.plot(x,y,linestyle='dashed',color='k')
legend_str.append('walls')
# plot robot positions
if self.output_real!='':
plt.plot(self.real[:,0],self.real[:,1],marker='o',color='g')
legend_str.append('real')
# plot visual positions
if self.output_vis_fix!='':
if len(self.vis_fix.shape) > 1:
plt.plot(self.vis_fix[:,0],self.vis_fix[:,1],marker='x',color='r')
else:
plt.plot(self.vis_fix[0],self.vis_fix[1],marker='x',color='r')
if zoom:
error = np.linalg.norm(self.real[:,:2]-self.vis_fix[:,:2])/self.real.shape[0]
legend_str.append('visual fixed RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('visual fixed')
if self.output_vis_free!='':
if len(self.vis_free.shape) > 1:
plt.plot(self.vis_free[:,0],self.vis_free[:,1],marker='x',color='grey')
else:
plt.plot(self.vis_free[0],self.vis_free[1],marker='x',color='grey')
if zoom:
error = np.linalg.norm(self.real[:,:2]-self.vis_free[:,:2])/self.real.shape[0]
legend_str.append('visual free RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('visual free')
# plot odometry positions
if self.output_enc!='':
pos_0 = [round(self.real[0,0]),round(self.real[0,1]),THETA_0]
self.odo = self.odometry(pos_0)
plt.plot(self.odo[:,0],self.odo[:,1],marker='x',color='k')
if zoom:
error = np.linalg.norm(self.real[:,:2]-self.odo[:,:2])/self.real.shape[0]
legend_str.append('odometry RMS-error = {0:3.2f}'.format(error))
else:
legend_str.append('odometry')
# plot expected robot movement
if self.output_mov!='':
plt.plot(self.mov[:,0],self.mov[:,1],marker='o',markersize=2,color='k')
error = np.linalg.norm(self.real[:,:2]-self.mov[:,:2])/self.real.shape[0]
legend_str.append('expected RMS-error = {0:3.2f}'.format(error))
# plot camera positions
if self.output_cam!='' and not zoom:
cam_wall = []
# first time: change to wall reference frame.
if self.camwall=='':
for c in self.cam:
res=calib.change_ref_to_wall(PTS_BASIS,MARGIN,c[1:].reshape(1,3))
c[1:]=res[0]
cam_wall.append(c)
self.camwall = np.array(cam_wall)
colors={139:'blue',141:'red',143:'green',145:'orange'}
for i in range(0,self.camwall.shape[0]):
plt.plot(self.camwall[i,1],self.camwall[i,2],marker='x',linestyle='',color=colors[self.camwall[i,0]])
#legend_str.append(str(int(self.camwall[i,0])))
if self.output_camreal!='' and not zoom:
colors={139:'blue',141:'red',143:'green',145:'orange'}
for i in range(0,self.camreal.shape[0]):
plt.plot(self.camreal[i,1],self.camreal[i,2],marker='o',linestyle='',color=colors[self.camreal[i,0]])
#legend_str.append(str(int(self.camreal[i,0])))
plt.legend(legend_str,'best')
# plot properties
plt.grid(True)
plt.xlabel('x [mm]'),plt.ylabel('y [mm]')
if zoom:
#plt.xlim([2000,5000])
#plt.ylim([2000,5000])
ax=plt.gca()
ax.relim()
ax.autoscale_view()
plt.axes().set_aspect('equal')
plt.title('Experimental Results - Robot Positions\n')
else:
plt.axes().set_aspect('equal')
plt.xlim([-1000,8000])
plt.ylim([-1000,8000])
plt.title('Experimental Results in Room Reference Frame \n')
plt.show(block=False)
if fname!='':
plt.savefig(self.out_dir+fname)
cam.reposition(img.ref_obj,img.ref_img,flag)
ref, err_img,lamda = cam.check_imagepoints(img.augment(img.ref_obj),
img.ref_img)
#--- Individual Robot position ---#
img.r_obj,err2,err3 = calib.get_leastsquares([cam],[img.augment(img.r_img)],
'hz',R_HEIGHT,r_real)
imgs[i]=img
cams[i]=cam
pts[i]=img.augment(img.r_img)
pts_ref[i]=img.augment(img.ref_img)
errs[i]=err_img
p_lq_fix,p_lq_free=leastsquares(choice_ref,pts_ref,cams,errs,img,pts,r_real,loop_counter,TIME)
name='posobj_fix_'+str(choice_ref)+'_'+TIME+'.txt'
p_lq_fix_wall = calib.change_ref_to_wall(PTS_BASIS,MARGIN,p_lq_fix.T[0])
calib.write_pos(out_dir,name,p_lq_fix_wall[0])
name='posobj_free_'+str(choice_ref)+'_'+TIME+'.txt'
p_lq_free_wall = calib.change_ref_to_wall(PTS_BASIS,MARGIN,p_lq_free.T)
calib.write_pos(out_dir,name,p_lq_free_wall[0])
#--------------------------- 4.2 Odometry Localization ------------------#
if DEBUG:
choice='n'
else:
choice = raw_input(ROBOT_ODO)
if choice == 'y':
# Connect to robot
if not robot_connected:
Robot = move.Robot()
