rmse extraction and plotting added to exp script

scripts/experiments.jl

@@ -4,9 +4,10 @@
 
 using AutomotiveSimulator
 using AutomotiveInteraction
+using PGFPlots
 
 """
-function run_experiment(f;ts=1,id_list=[],dur=10.,modelmaker=make_IDM_models)
+function extract_metrics(f;ts=1,id_list=[],dur=10.,modelmaker=nothing,filename=[])
 
 - Perform driving simulation and extract metrics
 
@@ -15,34 +16,83 @@ function run_experiment(f;ts=1,id_list=[],dur=10.,modelmaker=make_IDM_models)
 - id_list: list of vehicles 
 - dur: duration
 - modelmaker: function that makes the models
+- filename: provide optionally to make comparison video
 
 # Example
 ```julia
 f = FilteringEnvironment()
-scenes,collisions = run_experiment(f,id_list=[4,6,8,13,19,28,29],
+scenes,collisions = extract_metrics(f,id_list=[4,6,8,13,19,28,29],
 modelmaker=make_cidm_models,filename="media/cidm_exp.mp4")
 ```
 """
-function run_experiment(f;ts=1,id_list=[],dur=10.,
-modelmaker=make_IDM_models,filename="media/run_exp.mp4")
+function extract_metrics(f;ts=1,id_list=[],dur=10.,modelmaker=nothing,filename=[])
     print("Run experiment from scripts being called \n")
     scene_real = deepcopy(f.traj[ts])
     if !isempty(id_list) keep_vehicle_subset!(scene_real,id_list) end
-
-    models = modelmaker(scene_real)
-
-    nticks = Int(ceil(duration/f.timestep))
+
+    # If modelmaker argument is provided, use that function to create models
+    # Otherwise, obtaine driver models using particle filtering
+    models = Dict{Int64,DriverModel}()
+    if isnothing(modelmaker)
+        print("Let's run particle filtering to create driver models")
+        models, = obtain_driver_models(f,id_list,30,1,30)
+    else
+        print("Lets use idm or c_idm to create driver models\n")
+        models = modelmaker(scene_real)
+    end
+
+    nticks = Int(ceil(dur/f.timestep))
     scene_list = simulate(scene_real,f.roadway,models,nticks,f.timestep)
-    
+
     c_array = test_collision(scene_list,id_list)
+
+    truth_list = f.traj[ts:ts+nticks]
 
-    truth_list = f.traj[start_frame:start_frame+nticks]
-    video_overlay_scenelists(scene_list,truth_list,id_list=id_list,roadway=f.roadway,
-    filename=filename)
+    # Make a comparison video if filename provided
+    if !isempty(filename)
+        video_overlay_scenelists(scene_list,truth_list,id_list=id_list,roadway=f.roadway,
+            filename=filename)
+    end
 
-    return scene_list, c_array
+    # rmse dict with vehicle wise rmse values
+    rmse_pos_dict,rmse_vel_dict = compute_rmse(truth_list,scene_list,id_list=id_list)
+
+    # Average over the vehicles
+    rmse_pos = rmse_dict2mean(rmse_pos_dict)
+    rmse_vel = rmse_dict2mean(rmse_vel_dict)
+
+    return rmse_pos,rmse_vel,c_array
 end
 
 f = FilteringEnvironment()
-scenes,collisions = run_experiment(f,id_list=[4,6,8,13,19,28,29],
-modelmaker=make_cidm_models,filename="media/cidm_exp.mp4")
+veh_id_list = [4,6,8,13,19,28,29]
+
+print("Get the metrics for particle filering\n")
+pos_pf,vel_pf,collisions_pf = extract_metrics(f,id_list=veh_id_list)
+
+print("Get the metrics for c-idm\n")
+pos_cidm,vel_cidm,collisions_cidm = extract_metrics(f,id_list=veh_id_list,
+modelmaker=make_cidm_models)
+
+print("Get the metrics for idm\n")
+pos_idm,vel_idm,collisions_idm = extract_metrics(f,id_list=veh_id_list,
+modelmaker=make_IDM_models)
+
+make_plots=false
+if(make_plots)
+    # Make rmse_pos plot
+    p_pos_pf = PGFPlots.Plots.Linear(collect(1:length(pos_pf)),pos_cidm,legendentry="pf");
+    p_pos_cidm = PGFPlots.Plots.Linear(collect(1:length(pos_cidm)),pos_cidm,legendentry="cidm");
+    p_pos_idm = PGFPlots.Plots.Linear(collect(1:length(pos_idm)),pos_idm,legendentry="idm");
+    ax_rmse_pos = PGFPlots.Axis([p_pos_pf,p_pos_cidm,p_pos_idm],
+            xlabel="timestep",ylabel="rmse pos",title="rmse pos: c-IDM vs IDM");
+    PGFPlots.save("media/rmse_pos_cidmvsidm.pdf",ax_rmse_pos);
+
+    # Make collisions plot
+    coll_pf = PGFPlots.Plots.Linear(collect(1:length(collisions_pf)),collisions_pf,legendentry="pf")
+    coll_idm = PGFPlots.Plots.Linear(collect(1:length(collisions_idm)),collisions_idm,legendentry="idm");
+    coll_cidm = PGFPlots.Plots.Linear(collect(1:length(collisions_cidm)),collisions_cidm,legendentry="cidm");
+    ax_coll = PGFPlots.Axis([coll_pf,coll_cidm,coll_idm],
+            xlabel="timestep",ylabel="iscolliding",title="Collision assessment: c-IDM vs IDM");
+    PGFPlots.save("media/coll_cidmvsidm.pdf",ax_coll)
+end

src/Filtering/particle_filtering.jl

@@ -176,7 +176,7 @@ function multistep_update(f::FilteringEnvironment;car_id,start_frame,last_frame,
     push!(iterwise_p_set,p_mat)
 
     for framenum in start_frame:last_frame
-        print("frame number = $(framenum)\n")
+        #print("frame number = $(framenum)\n")
         scene = deepcopy(f.traj[framenum])
         true_next_scene = deepcopy(f.traj[framenum+1])
         true_nextpos = get_frenet_s(true_next_scene;car_id=car_id)
@@ -202,7 +202,7 @@ function multistep_update(f::FilteringEnvironment;car_id,start_frame,last_frame,
 end
 
 """
-    function obtain_driver_models(veh_id_list,num_particles,start_frame,last_frame)
+    function obtain_driver_models(f::FilteringEnvironment,veh_id_list,num_particles,start_frame,last_frame)
 Driver models for each vehicle in veh_id_list
 
 # Arguments

src/Filtering/utils.jl

@@ -326,7 +326,7 @@ function compute_rmse(true_scene_list,imit_scene_list;id_list=[])
         demo_scene_target = true_scene_list[i]
 
         for veh_id in id_list
-            print("vehid = $veh_id\n")
+            #print("vehid = $veh_id\n")
             demo_veh = demo_scene_target[findfirst(veh_id,demo_scene_target)]
             ego_veh = scene_halluc[findfirst(veh_id,scene_halluc)]
 
@@ -370,6 +370,7 @@ function rmse_dict2mean(rmse_dict)
     end
 
     carmean_rmse = mean(rmse_array,dims=2)
+    carmean_rmse = reshape(carmean_rmse,length(carmean_rmse),)
     return carmean_rmse
 end