From c5c2ce11180ce0642d4e13895adb050802d2b35d Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Sat, 8 Sep 2018 19:28:26 -0700
Subject: [PATCH] minor changes on graphs

---
 .../__pycache__/data_analyzer.cpython-36.pyc  | Bin 5482 -> 5441 bytes
 .../__pycache__/realtime_plot.cpython-35.pyc  | Bin 6555 -> 6464 bytes
 .../__pycache__/realtime_plot.cpython-36.pyc  | Bin 5905 -> 5818 bytes
 CoLo-AT/data_analysis/data_analyzer.py        |   4 +-
 CoLo-AT/data_analysis/realtime_plot.py        |  33 +++++----
 .../__pycache__/state_recorder.cpython-36.pyc | Bin 4628 -> 4624 bytes
 CoLo-AT/simulation_process/state_recorder.py  |   2 +-
 CoLo-AT/test_simulation.py                    |  66 ++++--------------
 8 files changed, 35 insertions(+), 70 deletions(-)

diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index 56fb8249090c574284edf5a20dfca7d00694b64c..c1276daac2b6a95b0506869361272989136596e3 100644
GIT binary patch
delta 574
zcmY+>-)qxQ6bJC!q}@nnSGz{X(7_ZzQm9o2Zqsc#+z&TpFDp{1GsN|xwM`>usXriN
zFN5Lp^-mB%1;^-{PwwBiFMIBu_u`YDZ}wy$A5Korx#!;Gq;yoWCv!RHc<*!l*P%Tj
zF3nV4N1?2|Z?@abAgnx$o<u3-g=()Weq@C1T^ZQ7gr~mQKZRB`=Nh}?jE(7wF$G)F
ze=Et4o1yfZv7~oFTlWr}uh#C0VHorXtb#R0Tb*uL)kC2Sw!0)l*@?-*%e1zP2o3!-
zhpv7WIkff%5IqJa<7Liz{$IcUmoIOSZg9mjs-XBLqixA^B?Y{{9Fn)dZN@9mFycF4
z3e14J;2xL-7)Wzqp3$nDpWsihp1;iBrj<VxHg@g_+Avg-nN?p3ey)y;d3LzEIzRHq
zO*r$)*s=J|e0Z9OMPqYajTc7?X2WQi+tW$*jY!PaAg!sn;!1Xz*PB+KidSn(+(_pD
zc1)At1>@Q2-gO}$@*6=M_$0R?YDm&f)M<b8)4xN{QNIb&L*7TUz-VQ;55$OCbg$bb
aey;MsWfi+q7d(`tQ)`GYi(abV?)QIw2aHSr

delta 637
zcmY+B%Wl(95Qcs1xWNXuijsmVK&lW@>#D}w3#CA4ld@=vkP-?Qkg<|eaPS39oKTQm
zmaJHvS3qo#Dxo|9ui*{ruwuam2?=K06_)&IJm>%Ca{O)S$C7r*F!cSr`Q+1AZJxFI
z_RBcr_8UJ4d@r=yemD%g#E&DZ8I631qmW05wHt%A5y!7N;Ms^<t+?w2{<}hG?+&9b
zIR@hf`%+@g_^0|oVT)o<J7A7j(9a@U`rMpzJOQuEU993Je#l)v;V|+7XS}a}R^G2m
zk^~!s2BC?_m1r1t@}nX521DR93Mh0~XqPD>!=%WOYobz)tdl8<HVHDqEFF6OuRr;V
zk68u_6_4Q@iETu#aqLUGaxf@)o^SzSR$!XqD}<|rYlQ2BRl*GdHH0<7O@t%<OwZ#L
zoSr#_ce6V4i)l2u$zWR&TctHIsJKR(GV<YaZSlSG`*MPr7gDC+TiUyqvWE0*GgDKk
zFQsaa_2iH8Qze_q%Gr@r?Xy&VRfcs@tKL;RSb0S}ubz2S#}aUiK(oU|!VbdJiY=Uq
z2jKR+#B+gnW9V^k^f(IMy2XEj4(T5digj;MbQ|G_yjo|vI9XfJO#G&Xcf?k0Wp<Gy
Sb;5lD^@4}Om^~p5YT_Sb_MW)_

diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc
index 3d9551659bc133328d33f6bc01e68622869d0d57..60ef9783aab84de2bb519a65425e593e61c2875b 100644
GIT binary patch
delta 922
zcmZ9KO-~b16o%hBZNat_rXTdXP&BA0r3!x0oq<F{02i9LP(ubzX=i9NGF*j_nbf2)
zE`^Lceu67Q;>v|f|A9&W0Xw&DjQ3uG7Bb11bDsOY=iHp!qq*<1;dCq(dGCFCvH2xj
z(+cy|rroYrcg<GIG}_e#d)Zc@^6=U5N9|{X>Bo)OAB`ROoB9)W&;P0~r|u)yW$>cl
z1F(kRMnDhzbbO&hQBWLR#+Vy}H3CZDJkmkQeiVf@3`$)<!Z>;X31hk+4Jl*hJnG1i
z+4Ia2&Riejak!V@4ubON2H*|jKPf08dK5t=&=_bOR7N8NPX|vZ<1k!Z$>L{Z<cuWd
zz>`W8gR69MhA=1lxKfjtBzOuu<v&W)IzO}q;AJ6W4tq7Y0a&Bx0*kMv@lu1ASFr%E
z2saBW4Og^aU~WzoMOAFS)t<}6XQW1)&;;l*=n7~aGzq$jt`=ZioT&|TMc)#54jH#3
z{oWXO9##Qv7+kF2nx9F|cCLe$a4MiF&<)T{(6s0j^v1!*<R!f_+%hb&tjg)7#2w0@
z8Bi5;3v?Sa3z`$sfO;6>><;Lzs2P(_XhJq&1U#Lpt@Z{t&Bl(oUq2ZFaN_??u1uyx
z<}exZBJ@J_?KehiKP(?>d9mKMoyP+EYpLGW{@`AvWjKv`FKFzUJH6n#&6~Z@ZqwWl
zzqVo7PA{mskH}8pma$%MsX8L+Da|p&&sjgs)kGNt|2JLaw$w2H*XZYv!$s~NrQao@
u+}zk&du=#ITa4Xl+PlXynI#ravJ0R8AzK+JYI?vq^uJ{f*rC6YTm1`#w#3N*

delta 980
zcmZWnO-~b16g@L7NP(8pmeTTB@v~^Df}kKu44M$rU<?ZvCX%6dTBc4rW%>%FnTiQv
z;Z8Fv^%oddhPdZHm}oM4L)`hXH{KaOl=vod-#zD@^X|JZZ*TnVL?9jvw(os>@woFt
zU`EcBva3c}&2DJAt|{g0l2)$kiqMQo=3Zr8GmT1Ft%yv)Ff;RpQB_S|TURqVBd_RM
z!xLr;^-A7#)OMf9U)rV4-KpSjS$bwqh95{b?458fHjU)Agk}dsD@vWP+Cek+*YNxn
zvyv4=sSDH%4|Jdu1VxTP2oyaAEQ=k0PEXcz3|JOF1}y9KfC$5CgLMj&z_t&~5dJbr
zX135|Z6Bx~Gyoa|4WZbAW*DN|bB17rJ!kh3({;o|AR?Y72+NbXHP{>dhxf$9m6(q~
zL_K{6EWUykSgo+N$Y^{=E&|O2+?d4X3@jf?G5cMlU+sa_f6!GDB?bc?SeuX^xtA7)
zm4KB54TDmk)1aH6GoZ8BmVJ`wJ?wp3MspB?rGu`$IRwGG^}z~2aNp<bbaZ^{JjB4E
zfG&V8g3_Q--pJn^hF~KfE(I%v(h%s<!I}67T=}xgQU){&x&rcnu7W~r@Jrn13B|4d
z6z+RSu{OicU>GHCVFGjwGzpplUFSNoM8Hz|_M&sDpcQLcLp2xd&(Vj;5CBthLm!7m
zVBW|Xsk>^~uyZkI%W0{V6<v4yRnw?Cp_LV5b6FV6N+rK)nEP*DyW_{)K0A4QpEIL8
zIsQuC(Dx_2@oTOTD%5p-S*SHJJyz5TPV0uL3DtBuw6aoE`PcGK9bchZ`!>t&v}`W$
zWGZfMul4i|m0jqbkeBLizJ8Ezdb$0o=e>O={yf^Mt3|a!j=#2QY;aW8)!k3=`%+g>
bI`P?KiA>vwG$Oalrfn;Umy&J&Of3EZHh1W7

diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index 366ecb8d007190f9aa9ccc357bae5995364e12c0..d60b97bfd306ab8c036651f323b5266f1c646b1d 100644
GIT binary patch
delta 688
zcmZ9J%WD%s9LHxi+0<>4eI%PSo9DJZYHZWs<DxgQ2Op(S6r`xgHorBy>xQ+PDG6a0
z3ya5qvi<{}ig=RTM7($t|AIZ}QF1PL@Z@Z$CCD(p`F_6NVV-X`KDD^3+3eyg|Lvn^
zAGx2na=mRhJ-t0JZQIm(?OV>4Gb^o|4<^@`FB}#p$LV7R55r&l`<Ea>ddy(tWi0`u
zIT9gvN@<IS{2U1&&yfg)Q_9ep#W@mSX^uoF{UaiIXS@a2Rf2@ipFo*v0V+_18q}fD
zWqpz4=rrCFX(G+EFw;4br+zk0KnYksf5J3{GG&>_;SF)E`@4($679-x!%J9&0Bxct
zD~}n!Onb$zdL^sqRRT<}uTWKu)oZ-}`C<GVOJ2!SK!wY&0%yl8Mx+!UIbi%6QM_7!
z{W>XI6^|p;@VmIYRf|112j}6!IOR7;JwU$dsa6A4XFFsq6)r*(F2UM3F?(<o)}b|~
zdy<s393YGU-ABzU+tW<ABkius2%#o+@6f@;aVgUhvHJdjW{<e|U)4>$=eW129e$I3
z+#4+nTejwQ^(eXL5F=uHn&r4tf#%Ibl+=c1KVk=l*)gJI-yG@F#W=wcXKQ=99Xn0N
z?NZM*!qt2|br4G&cJd!s_mJw4mk)DkVs@TCeWAHpuTPgToWbOsd>2bR{;!393aymL
RWD@RC*e-_nDEwO7{tIb0wz>cS

delta 771
zcmY+AO=}ZD7{_-uFD6a)CE27++N4c=0b|?-ttf)fLoJk2dg(#LL)qrpq`U6!q?<`w
z$}ClK@(`))N$(ztCn<_3h<MU(AbZeb@uu_(IJ*s~&oIO9`Tw7pd1l_L&*swef-v^!
z&65Y&*XbW<)ogBITW{_fmSt#mbKS7}mPQQhs7ua{;bO<u9iq0et1e>P)m`T8=;|tN
zX_m1c6*k*_rxot#9mn{OX_P(woH^yurvFwvIv60tQNffzjwoXakQ<Q*`4LH=M3iN2
zVMHPnM<l^`NJOS7dJZNAF;5(EiLnG_n1m^)K(&+bWRhc5B9)nwW86qlDN6Dr&kP~7
z*bxa&8Rg7{63Yuj@o&iu=}U+6N^F+*=4i|;_}j9wRHRc;D`m4tD<K`C?Wgy`XN}Vm
zEyFZigBr{Z6RgRJu*m`MRY{3f2gsWyWpk3I$&~+9o`p)N!Fjj<7l%o2hExZfSEDs^
z8ZJdQ2<8l}!FAw4fXl-;`?_qT%>PwL(yR?Q=FFJ0a0ME071Uu2alox}^&4yMvVSvo
zABp~9u8`SJFXB~PyQ|yS|B(BB5F~oGW?3gMo`>;!;e?Cg*FrrW#Jeu;2IA%>-rFE}
zLvvbN*mV_F5X7Apwoc}wI9=Em+kMO0AbOA7p6eLxAY<5CM`zP$J^XoxZK(4>a@RG8
z?gp`Tw^!%gWv05FGtWg~5-!Y$c=C*U|L8Pi?@WIjv7b_2`R|lRO48Chx&uMHw}p4v
ks_p2<D}@y#jiLXB|FWp2@+i-zc=yQvR`k)4Z;h}00iR3Pu>b%7

diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index 2eb2c31..a41061c 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -217,8 +217,8 @@ class Analyzer():
 		arr_loc_err = []
 		arr_trace = []
 		for data_recorder in arr_data_recorder:
-			loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = True)
-			if data_recorder.name not in only_trace:
+			loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False)
+			if only_trace ==None or data_recorder.name not in only_trace:
 				arr_loc_err.append([time_stamps, loc_err_per_run, data_recorder.name])
 			arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
 
diff --git a/CoLo-AT/data_analysis/realtime_plot.py b/CoLo-AT/data_analysis/realtime_plot.py
index 436e6fc..4ae8f2c 100644
--- a/CoLo-AT/data_analysis/realtime_plot.py
+++ b/CoLo-AT/data_analysis/realtime_plot.py
@@ -95,7 +95,7 @@ def update(i, num_robots, fig, l_est, l_gt, scat_est, scat_gt, location_est, loc
 	res = []
 	for txt in fig.texts:
 		txt.set_visible(False)
-	tmp = fig.text(0.8, 0.03, "Time: " + str(round(times[0][i], 1)), fontsize=12)
+	tmp = fig.text(0.8, 0.03, "Time: " + str(round(times[0][i], 1)), fontsize=16)
 	for robotNum in range(0, num_robots):
 		# factor out common variables
 		est_x = location_est[robotNum]['x']
@@ -157,7 +157,7 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
 	fig = plt.figure(figsize=(12, 9), tight_layout=True)
 	fig.subplots_adjust(top=0.88)
 	gs = gridspec.GridSpec(3, 2)
-	fig.suptitle('CoLo Robot Demo Simulation', fontsize=14)
+	fig.suptitle('CoLo Robot Demo Simulation', fontsize=20)
 	ax_err = plt.subplot(gs[0, 0])
 	ax_var = plt.subplot(gs[0, 1])
 	ax = plt.subplot(gs[1:, :])
@@ -169,15 +169,16 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
 	#ax = plt.axes(xlim=(-6, 6), ylim=(-6, 8))
 	#ax_err = plt.axes(xlim=(0, 100), ylim=(0, 0.4))
 	# assign labels
-	ax.set_title('CoLo Demo')
-	ax.set_xlabel('x-axis[m]')
-	ax.set_ylabel('y-axis[m]')
-	ax_err.set_title('Estimation deviation error')
-	ax_err.set_xlabel('Time[s]')
-	ax_err.set_ylabel('RMS[m]')
-	ax_var.set_title('Trace of state variance')
-	ax_var.set_xlabel('Time [s]')
-	ax_var.set_ylabel('Sigma_s [m^2]')
+	ax.axis('equal')
+	ax.set_title('CoLo Demo', fontsize=16)
+	ax.set_xlabel('x-axis[m]', fontsize=16)
+	ax.set_ylabel('y-axis[m]', fontsize=16)
+	ax_err.set_title('Estimation deviation error', fontsize=16)
+	ax_err.set_xlabel('Time[s]', fontsize=16)
+	ax_err.set_ylabel('RMS[m]', fontsize=16)
+	ax_var.set_title('Trace of state variance', fontsize=16)
+	ax_var.set_xlabel('Time [s]', fontsize=16)
+	ax_var.set_ylabel('Sigma_s [m^2]', fontsize=16)
 	# obtain robot locations from dataset
 	robot_locations_est = get_robot_locations(num_robots, robot_loc_time_unit, 'est')
 	robot_locations_gt = get_robot_locations(num_robots, robot_loc_time_unit, 'gt')
@@ -208,10 +209,12 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
 	ani = animation.FuncAnimation(fig, update, fargs = (num_robots, fig, init_path_est, init_path_gt, init_points_est, init_points_gt, robot_locations_est, robot_locations_gt, times, time_func, min_time_arr, arr_err, arr_var, loc_err, trace_sigma), frames=min_length(times), interval=50, blit=False)
 	# Show legend
 	fontP = FontProperties()
-	fontP.set_size('small')
-	ax.legend(prop=fontP, bbox_to_anchor=(1.1, 1.0), loc=9, ncol=1)
-	ax_var.legend(prop=fontP, bbox_to_anchor=(1.2, 1.0), loc=9, ncol=1)
+	fontP.set_size('x-large')
+	ax.tick_params(axis='both', which='major', size=15)
+	
+	ax.tick_params(labelsize=10)
+
 	# Show graph
 	plt.show() 
 	# (optional) save as GIF, comment the following line if you don't want this
-	ani.save('full_test9.gif', writer="imagemagick", fps=60)
+	#ani.save('full_test9.gif', writer="imagemagick", fps=60)
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc
index 79b36539455ad2d99eb5288bc0f185b944a12294..b4fff0660725a669b0823d3b42ca96f078fc881a 100644
GIT binary patch
delta 29
lcmbQDGC_sYn3tDJr+G@Wz(&q_?5sti3=9mLH?XI00RV2!2R;A*

delta 33
lcmbQBGDU^cn3tF9+pmeyLK`{fv2&&~fWZU%&1=|GxB#dC34H(n

diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 245117c..01028df 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -68,7 +68,7 @@ class StatesRecorder():
 			print('neg trace: ', recorded_dataline)
 		
 
-		if(loc_err >= 0.5):
+		if(loc_err >= 1):
 			print(updata_type)
 			print('>1 m loc err: ',recorded_dataline)
 			print(req.get_message())
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 97c4c37..79192ed 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -30,67 +30,29 @@ from ekf_gs_ci2 import EKF_GS_CI2
 dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
 
 dataset_labels = [1,2,3]
-duration = 150 # duration for the simulation in sec
+duration = 300 # duration for the simulation in sec
 testing_dataset = Dataset('testing')
-start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
+analyzer = Analyzer('analyzer', dataset_labels)
 
-freqs0 = [[10, 10, 10],[1, 1, 1],[0.5, 0.5, 0.5]]
-freqs1 = [[10, 10, 10],[4, 4, 4],[0.5, 0.5, 0.5]]
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
 
 loc_algo = EKF_GS_CI2('algo')
 robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
 
-sim = SimulationManager('sim EKF_GS_CI2')
-state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder, freqs0, simple_plot = True)
-#sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
-
-analyzer = Analyzer('analyzer', dataset_labels)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
-robot_loc_time_unit = analyzer.robot_location_at_unit_time_interval(state_recorder)
-data_in_time_order = state_recorder.get_data_in_time_order()
-update_in_time_order = state_recorder.get_updata_type_in_time_order()
-
-'''
+sim = SimulationManager('sim EKF_GS_CI')
+state_recorder = StatesRecorder('EKF_GS_CI',dataset_labels)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
 ##############################################################################
-testing_dataset.dataset_reset()
-robot = RobotSystem('robot gs ci', dataset_labels, loc_algo, distr_sys = True)
-
-sim1 = SimulationManager('sim')
-state_recorder1 = StatesRecorder('gs ci schedule freq1',dataset_labels)
-sim1.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder1, freqs1, simple_plot = True)
 
-##############################################################################
 testing_dataset.dataset_reset()
-robot = RobotSystem('robot gs ci', dataset_labels, loc_algo, distr_sys = True)
-
-sim_n = SimulationManager('sim')
-state_recorder_n = StatesRecorder('gs ci naive ',dataset_labels)
-sim_n.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder_n, simple_plot = True)
-
-analyzer = Analyzer('analyzer', dataset_labels)
-analyzer.algos_comparison([state_recorder, state_recorder1, state_recorder_n], only_trace=['gs ci bound'])
-print('done!')
-'''
-'''
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder)
-#robot_location_at_unit_time_interval
-robot_loc_time_unit = analyzer.robot_location_at_unit_time_interval(state_recorder)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
 
+sim_n = SimulationManager('Centralized_EKF')
+state_recorder_n = StatesRecorder('Centralized_EKF ',dataset_labels)
+sim_n.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder_n, simple_plot = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_n, plot_graphs = True)
 
-# simulated data
-data_sim = state_recorder.get_recorded_data()
-le = state_recorder.get_loc_err_arr()
-ud = state_recorder.get_update_type_arr()
-t_S = state_recorder.get_trace_sigma_s_arr()
-ud_t = state_recorder.get_updata_type_in_time_order()
-data_sim_t = state_recorder.get_data_in_time_order()
-get_robot_time_func = state_recorder.get_time_arr
-# le, t_S is an array of 5 elements, each element has error, state variance 
-# based on time stamp
-# print(len(t_S[1]), len(t_S[2]), len(t_S[3]), len(t_S[4]), len(t_S[5]))
-# print(data_sim)
-'''
+analyzer.algos_comparison([state_recorder, state_recorder_n])
 
-#animate_plot(dataset_labels, state_recorder, analyzer, get_robot_time_func, le, t_S)
-- 
GitLab