work on analysys scripts

bag/analysys/.gitignore

+*.asv
+*.jpg

bag/analysys/compare.m

@@ -12,6 +12,7 @@ if ~isfolder(CSVFolder)
 end
 Gvec = [];
 Evec = [];
+Tvec = [];
 myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in struct
 for k = 1:length(myFiles)
     baseFileName = myFiles(k).name;
@@ -24,27 +25,83 @@ for k = 1:length(myFiles)
     elseif baseFileName(1) == 'E'
         [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table);
         Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
+    elseif baseFileName(1) == 'T'
+        [Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table);
+        Tvec = [Tvec, {baseFileName, Tx, Ty, Tz, Tang, Tt, Tts}'];
     end
-
-%     
-%     t10 = linspace(0,min(Gt(end),gt10(end))-1,1e6);
-%     ts0 = resample(Gts, t10);
-%     ts1 = resample(gts10, t10);
-%     ploterrs(ts0, ts1)
-%     
-%     GC20 = readtable('trajectory_ana_lab_2_exp_20_transp.csv','PreserveVariableNames',true);
-%     [gx20, gy20, gz20, gth20, gt20, gts20] = extractdata(GC20);
-%     
-%     t20 = linspace(0,min(Gt(end),gt20(end))-1,1e6);
-%     ts0 = resample(Gts, t20);
-%     ts2 = resample(gts20, t20);
-%     ploterrs(ts0, ts2)
 end
+
 ERRORS=[];
-min_len = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
-for i = 1:size(Gvec,2)
-    [perrors, angerrors] = geterror(Gvec{7,i}, Evec{7,i});
-    Initial_RMSE = [sqrt(mean(perrors.^2)), sqrt(mean(angerrors.^2))];
-    ERRORS = [ERRORS, {strcat(Gvec{1,i},' ', Evec{1,i}), Initial_RMSE, perrors, angerrors}']
-    min_len = min(min_len, length(perrors));
+min_len2d = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
+min_len3d = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
+for i = 1:size(Evec,2)
+    [perrors2d, angerrors2d] = geterror(Gvec{7,i}, Evec{7,i});
+    [perrors3d, angerrors3d] = geterror(Gvec{7,i}, Tvec{7,i});  
+    Initial_RMSE2d = [sqrt(mean(perrors2d.^2)), sqrt(mean(angerrors2d.^2))];
+    Initial_RMSE3d = [sqrt(mean(perrors3d.^2)), sqrt(mean(angerrors3d.^2))];
+    ERRORS = [ERRORS, {strcat(Gvec{1,i},' ', Evec{1,i}, ' ', Tvec{1,i}), Initial_RMSE2d, perrors2d, angerrors2d, Initial_RMSE3d, perrors3d, angerrors3d}']
+    min_len2d = min(min_len2d, length(perrors2d)); 
+    min_len3d = min(min_len3d, length(perrors3d));
+
+    figure('visible','off');
+    plot(linspace(0,366, size(ERRORS{3,i},2)), ERRORS{3,i})
+    title(strcat('2D Position error for ', ERRORS{1,i}))
+    xlabel('seconds')
+    ylabel('meters')
+    
+    figure('visible','off');
+    plot(linspace(0,366, size(ERRORS{4,i},2)), ERRORS{4,i})
+    title(strcat('2D Angular error for ', ERRORS{1,i}))
+    xlabel('seconds')
+    ylabel('radians')
+
+    figure('visible','off');
+    plot(linspace(0,366, size(ERRORS{6,i},2)), ERRORS{6,i})
+    title(strcat('3D Position error for ', ERRORS{1,i}))
+    xlabel('seconds')
+    ylabel('meters')
+    
+    figure('visible','off');
+    plot(linspace(0,366, size(ERRORS{7,i},2)), ERRORS{7,i})
+    title(strcat('3D Angular error for ', ERRORS{1,i}))
+    xlabel('seconds')
+    ylabel('radians')
+end
+
+ploterrorstiled
+
+for i = 1:size(ERRORS,2)
+    vecp2d = ERRORS{3,i};
+    rate2d = min_len2d/length(vecp2d);
+    if rate2d ~= 1
+        outliers = floor(linspace(1/min_len2d, 1/rate2d, length(vecp2d)-min_len2d)*min_len2d);
+        vecp2d(outliers) = [];
+    end
+
+    veca2d = ERRORS{4,i};
+    rate2d = min_len2d/length(veca2d);
+    if rate2d ~= 1
+        outliers = floor(linspace(1/min_len2d, 1/rate2d, length(veca2d)-min_len2d)*min_len2d);
+        veca2d(outliers) = [];
+    end      
+
+    Final_RMSE2d = [sqrt(mean(vecp2d.^2)), sqrt(mean(veca2d.^2))];
+
+    vecp3d = ERRORS{6,i};
+    rate3d = min_len3d/length(vecp3d);
+    if rate3d ~= 1
+        outliers = floor(linspace(1/min_len3d, 1/rate3d, length(vecp3d)-min_len3d)*min_len3d);
+        vecp3d(outliers) = [];
+    end   
+
+    veca3d = ERRORS{7,i};
+    rate3d = min_len3d/length(veca3d);
+    if rate3d ~= 1
+        outliers = floor(linspace(1/min_len3d, 1/rate3d, length(veca3d)-min_len3d)*min_len3d);
+        veca3d(outliers) = [];
+    end    
+
+    Final_RMSE3d = [sqrt(mean(vecp3d.^2)), sqrt(mean(veca3d.^2))];
+    ERRORS{8,i} = Final_RMSE2d;
+    ERRORS{9,i} = Final_RMSE3d;
 end
\ No newline at end of file

bag/analysys/ploterrorstiled.m

+a = findobj('Type','axes');
+m = 2;
+n = 2;
+for i = 1:size(Evec,2)
+    figure
+    for k=1:4
+        tmp1 = subplot(m,n,k);
+        tmp2 = copyobj(a(((i-1)*4)+k),gcf);
+        set(tmp2,'Position',get(tmp1,'Position'))
+        delete(tmp1)
+    end
+end
\ No newline at end of file

bag/generate_trajectory_csv2.bash

+#!/bin/bash
+#echo "Press CTRL+C to proceed."
+#trap "pkill -f 'sleep 1h'" INT
+#trap "set +x ; sleep 1h ; set -x" DEBUG
+
+if [[ -z "${BAG}" ]]
+then
+  BAG="ana_lab_2"
+fi
+
+#yamlfiles are divided into two types:
+#   Starting with G ==> groundtruth, we extract the trajectory
+#   Starting with E ==> experiment, we extract the poses
+#   Starting with T ==> experiment in 3D, we extract the poses
+timespanarray=("0.1" "0.5" "1.0" "2.0" "5.0" "10.0") 
+timespanarray=("0.1")
+for time_span in "${timespanarray[@]}"
+do
+  for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
+  do
+    echo "working on $yamlfile"
+    yamlname=${yamlfile%.*}
+    yamlname=$yamlname\-${time_span/./_}
+    sed -i "s/.*min_time.*/  min_time: $time_span/" ../yaml/trajectory_analysys/parameters/test_laser_processor.yaml 
+    sed -i "s/.*profiling_file.*/  profiling_file: \"~/profiling/wolf_demo_profiling_$yamlname\"/" ../yaml/trajectory_analysys/$yamlfile 
+
+    rm recordings/trajectory_recording$yamlname.bag
+    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=0.05 suffix:=$yamlname rviz:=false
+    if [[ ${yamlname:0:1} == 'G' ]]
+    then
+      tend=$(rosbag info -y -k end recordings/trajectory_recording$yamlname.bag)
+      tend=${tend%.*}
+      rosbag filter recordings/trajectory_recording$yamlname.bag recordings/trajectory_recording$yamlname\_tmp.bag "topic == '/wolf_ros_node/trajectory' and t.to_sec() >= $tend"
+
+      rm CSV/$yamlname.csv
+      rostopic echo -b recordings/trajectory_recording$yamlname\_tmp.bag /wolf_ros_node/trajectory -p > CSV/$yamlname.csv
+      rm recordings/trajectory_recording$yamlname\_tmp.bag
+
+      rm CSV/transp/$yamlname\_transp.csv
+      python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname.csv
+
+    elif [[ ${yamlname:0:1} == 'E' ]]
+    then
+      rm CSV/$yamlname.csv
+      rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+      cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
+    elif [[ ${yamlname:0:1} == 'T' ]]
+    then
+      rm CSV/$yamlname.csv
+      rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+      cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
+    fi
+  done
+  
+done

launch/imu2d_analysys.launch

 <!-- -->
 <launch>
-    <param name="use_sim_time" value="true" />
     <arg name="record" default="true" />
     <arg name="rviz" default="true" />
     <arg name="speed" default="1" />
     <arg name="sec" default="0" />
     <arg name="profiling" default="false" />
     <arg name="gdb" default="false" />
-    <arg name="test" default="" />
-    <arg name="bag" default="" />
+    <arg name="test" default="1" />
+    <arg name="bag" default="test_no_map" />
     <arg name="suffix" default="" />
     <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
     <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
     <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
-    <!-- using "clock" option to use the simulated time. Rosbag launch as the first node  -->
+    <arg name="robot" default="ana" /> <!-- ana or helena -->
 
-    <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+    <node pkg="rostopic" type="rostopic" name="" args='pub /imu_bno/enable std_msgs/Bool "data: True"'/>
+    <node pkg="rostopic" type="rostopic" name="" args='pub /imu_micro/enable std_msgs/Bool "data: True"'/>
+
+    <!-- RECORD -->
     <group if="$(arg record)">
-      <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+      <node pkg="rosbag" 
+            type="record" 
+            name="recorder" 
+            args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
     </group>
 
-    <node pkg="tf"
-          type="static_transform_publisher"
-          name="static_tf"
-      args="0 0 0 0 0 0 /ana/base_footprint /ana/base_link  100"/>
+    <!-- TF -->
+    <group>
+      <!-- <node pkg="tf"
+            type="static_transform_publisher"
+            name="static_tf"
+            args="0 0 0 0 0 0 /base_footprint /base_link  100"/>
 
-    <node pkg="tf"
-          type="static_transform_publisher"
-          name="static_tf2"
-      args="0 0 0 0 0 0 /ana/base_link /ana/imu_bno055  100"/>
+      <node pkg="tf"
+            type="static_transform_publisher"
+            name="static_tf2"
+            args="0 0 0 0 0 0 /base_link /imu_bno055 100"/>
 
-    <node pkg="tf"
-          type="static_transform_publisher"
-          name="static_tf3"
-      args="0 0 0 3.14159265 0 0 /ana/base_link /ana/velodyne  100"/>
+      <node pkg="tf"
+            type="static_transform_publisher"
+            name="static_tf3"
+            args="0 0 0 3.14159265 0 0 /base_link /velodyne  100"/> -->
 
-    <node pkg="tf"
-          type="static_transform_publisher"
-          name="static_tf4"
-      args="-0.11 0.01 0 0 0 0 /ana/base_link /ana/imu_microstrain  100"/>
+      <node pkg="tf"
+            type="static_transform_publisher"
+            name="static_tf4"
+            args="-0.11 0.01 0 0 0 0 /$(arg robot)/base_link /imu 100"/>
 
+      <node pkg="tf"
+            type="static_transform_publisher"
+            name="static_tf5"
+            args="-0.11 0.01 0 0 0 0 /$(arg robot)/base_link /$(arg robot)/imu_microstrain 100"/>
+    </group>
+
+    <!-- using "clock" option to use the simulated time. Rosbag launch as the first node  -->
+    <param name="use_sim_time" value="true" />
     <node pkg="rosbag"
     	  type="play"
     	  name="player"
         required="true"
-    	  args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>      
+    	  args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag">
+        <remap from="/$(arg robot)/sensors/imu/data" to="/imu_micro"/>
+        <remap from="/$(arg robot)/imu/data" to="/imu_micro"/>
+        <remap from="/$(arg robot)/sensors/bno055/imu" to="/imu_bno"/>
+        <remap from="/$(arg robot)/sensors/scan" to="/scan"/>
+        <remap from="/$(arg robot)/odom" to="/odom"/>
+        <remap from="/$(arg robot)/sensors/ublox_gps/raw_data_stream" to="/ublox_gps/raw_data_stream"/>
+    </node>      
 
+    <!-- RVIZ -->
     <group if="$(arg rviz)">
       <node type="rviz"
             name="rviz"
             pkg="rviz"
-            args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
+            args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo_$(arg robot).rviz"/>
     </group>
 
+    <!-- WOLF -->
     <node type="wolf_ros_node"
     	    name="wolf_ros_node"
     	    pkg="wolf_ros_node"

yaml/test_publisher_ros_node.yaml

 map_frame_id: "map"
-# odom_frame_id: "ana/odom"
-# base_frame_id: "ana/base_footprint"
-odom_frame_id: "helena/odom"
-base_frame_id: "helena/base_footprint"
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+  #odom_frame_id: "helena/odom"
+  #base_frame_id: "helena/base_footprint"
 publish_odom_tf: false

yaml/trajectory_analysys/E-0_1.yaml

-config:
-  debug:
-    profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-0_1.txt"
-    print_problem: false
-    print_period: 2
-    print_depth: 4
-    print_constr_by: false
-    print_metric: true
-    print_state_blocks: true
-  problem:
-    tree_manager:
-      type: "none"
-    frame_structure: "POV"
-    dimension: 2
-    prior:
-      mode: "fix"
-      $state:
-        P: [0,0]
-        O: [0]
-        V: [0,0]
-      time_tolerance: 0.1
-    node_rate: 100
-  map:
-    type: "MapBase"
-    plugin: "core"
-      
-  solver:
-    follow: "parameters/solver.yaml"
-
-    
-  sensors:
-    -
-      type: "SensorLaser2d"
-      name: "scanner_front_left"
-      plugin: "laser"
-      follow: "parameters/test_laser_params.yaml"
-    -
-      type: "SensorImu2d"
-      name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_params_bno.yaml" 
-    -
-      type: "SensorImu2d"
-      name: "microstrain"
-      plugin: "imu"
-      follow: "parameters/test_imu_params_microstrain.yaml" 
-
-  processors:
-    -
-      type: "ProcessorOdomIcp"
-      name: "processorodomicp"
-      sensor_name: "scanner_front_left"
-      plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 0.1
-        min_error: 999
-        max_points: 0
-      follow: "parameters/test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorLoopClosureIcp"
-        #  name: "processorloopclosureicp"
-        #  sensor_name: "scanner_front_left"
-        #  plugin: "laser"
-        #  time_tolerance: 0.1
-        #  apply_loss_function: true
-        #  keyframe_vote:
-        #    voting_active: false
-        #  recent_frames_ignored: 10
-        #  frames_ignored_after_loop: 0
-        #  max_error_threshold: 0.02
-        #  min_points_percent: 40
-        #  max_loops: 1
-        #  max_candidates: 5
-        #  max_attempts: 5
-        #  candidate_generation: "random" # 'random' or 'tree'
-        #  icp:
-        #    follow: "parameters/csm.yaml"
-    -
-      type: "ProcessorImu2d"
-      name: "processorimu2dbno"
-      sensor_name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_processor_bno.yaml"
-    -
-      type: "ProcessorImu2d"
-      name: "processorimu2dmicro"
-      sensor_name: "microstrain"
-      plugin: "imu"
-      follow: "parameters/test_imu_processor_microstrain.yaml"
-      
-  ROS subscriber:
-    -
-      package: "wolf_ros_laser"
-      type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
-      sensor_name: "scanner_front_left"
-      load_params_from_msg: true
-    #-
-    #  package: "wolf_ros_imu"
-    #  type: "SubscriberImuEnableable"
-    #  topic: "/ana/sensors/bno055_imu/imumal"
-    #  sensor_name: "bno"
-    #  follow: "parameters/test_imu_subscriber_bno.yaml"
-    -
-      package: "wolf_ros_imu"
-      type: "SubscriberImuEnableable"
-      topic: "/ana/imu/data"
-      sensor_name: "microstrain"
-      follow: "parameters/test_imu_subscriber_microstrain.yaml"
-
-  ROS publisher:
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTf"
-      topic: " "
-      period: 0.1
-      follow: "parameters/test_publisher_ros_node.yaml"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherGraph"
-      topic: "graph"
-      period: 0.1
-      viz_overlapped_factors: true
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTrajectory"
-      topic: "trajectory"
-      period: 0.1
-      frame_id: "map"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherPose"
-      topic: "pose"
-      extrinsics: false
-      period: 0.005
-      frame_id: "map"
-    -
-      package: "wolf_ros_laser"
-      type: "PublisherLaserMap"
-      topic: "map"
-      period: 1
-      map_frame_id: "map"
-      update_dist_th: 0.05
-      update_angle_th: 0.05
-      max_n_cells: 1000000
-      grid_size: 0.1
-      p_free: 0.3
-      p_obst: 0.8
-      p_free_th: 0.2
-      p_obst_th: 0.9
-      discard_max_range: true
-

yaml/trajectory_analysys/E-1_0.yaml → yaml/trajectory_analysys/E.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-1_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_E.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -52,14 +52,6 @@ config:
       name: "processorodomicp"
       sensor_name: "scanner_front_left"
       plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 1
-        min_error: 999
-        max_points: 0
       follow: "parameters/test_laser_processor.yaml"
         #-
         #  type: "ProcessorLoopClosureIcp"
@@ -97,19 +89,19 @@ config:
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
+      topic: "/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
     #-
     #  package: "wolf_ros_imu"
     #  type: "SubscriberImuEnableable"
-    #  topic: "/ana/sensors/bno055_imu/imumal"
+    #  topic: "/imu_bno"
     #  sensor_name: "bno"
     #  follow: "parameters/test_imu_subscriber_bno.yaml"
     -
       package: "wolf_ros_imu"
       type: "SubscriberImuEnableable"
-      topic: "/ana/imu/data"
+      topic: "/imu_micro"
       sensor_name: "microstrain"
       follow: "parameters/test_imu_subscriber_microstrain.yaml"
 

yaml/trajectory_analysys/G-0_1.yaml

-config:
-  debug:
-    profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_G-0_1.txt"
-    print_problem: false
-    print_period: 2
-    print_depth: 4
-    print_constr_by: false
-    print_metric: true
-    print_state_blocks: true
-  problem:
-    tree_manager:
-      type: "none"
-    frame_structure: "POV"
-    dimension: 2
-    prior:
-      mode: "fix"
-      $state:
-        P: [0,0]
-        O: [0]
-        V: [0,0]
-      time_tolerance: 0.1
-    node_rate: 100
-  map:
-    type: "MapBase"
-    plugin: "core"
-      
-  solver:
-    follow: "parameters/solver.yaml"
-
-  sensors:
-    -
-      type: "SensorLaser2d"
-      name: "scanner_front_left"
-      plugin: "laser"
-      follow: "parameters/test_laser_params.yaml"
-
-  processors:
-    -
-      type: "ProcessorOdomIcp"
-      name: "processorodomicp"
-      sensor_name: "scanner_front_left"
-      plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 0.1
-        min_error: 999
-        max_points: 0
-      follow: "parameters/test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorLoopClosureIcp"
-        #  name: "processorloopclosureicp"
-        #  sensor_name: "scanner_front_left"
-        #  plugin: "laser"
-        #  time_tolerance: 0.1
-        #  apply_loss_function: true
-        #  keyframe_vote:
-        #    voting_active: false
-        #  recent_frames_ignored: 3
-        #  frames_ignored_after_loop: 0
-        #  max_error_threshold: 0.02
-        #  min_points_percent: 40
-        #  max_loops: 3
-        #  max_candidates: 5
-        #  max_attempts: 15
-        #  candidate_generation: "random" # 'random' or 'tree'
-        #  icp:
-        #    follow: "parameters/csm.yaml"
-
-      
-  ROS subscriber:
-    -
-      package: "wolf_ros_laser"
-      type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
-      sensor_name: "scanner_front_left"
-      load_params_from_msg: true
-
-  ROS publisher:
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTf"
-      topic: " "
-      period: 0.1
-      follow: "parameters/test_publisher_ros_node.yaml"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherGraph"
-      topic: "graph"
-      period: 0.1
-      viz_overlapped_factors: true
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTrajectory"
-      topic: "trajectory"
-      period: 0.1
-      frame_id: "map"
-    -
-      package: "wolf_ros_laser"
-      type: "PublisherLaserMap"
-      topic: "map"
-      period: 1
-      map_frame_id: "map"
-      update_dist_th: 0.05
-      update_angle_th: 0.05
-      max_n_cells: 1000000
-      grid_size: 0.1
-      p_free: 0.3
-      p_obst: 0.8
-      p_free_th: 0.2
-      p_obst_th: 0.9
-      discard_max_range: true

yaml/trajectory_analysys/G-0_5.yaml

-config:
-  debug:
-    profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_G-0_5.txt"
-    print_problem: false
-    print_period: 2
-    print_depth: 4
-    print_constr_by: false
-    print_metric: true
-    print_state_blocks: true
-  problem:
-    tree_manager:
-      type: "none"
-    frame_structure: "POV"
-    dimension: 2
-    prior:
-      mode: "fix"
-      $state:
-        P: [0,0]
-        O: [0]
-        V: [0,0]
-      time_tolerance: 0.1
-    node_rate: 100
-  map:
-    type: "MapBase"
-    plugin: "core"
-      
-  solver:
-    follow: "parameters/solver.yaml"
-
-  sensors:
-    -
-      type: "SensorLaser2d"
-      name: "scanner_front_left"
-      plugin: "laser"
-      follow: "parameters/test_laser_params.yaml"
-
-  processors:
-    -
-      type: "ProcessorOdomIcp"
-      name: "processorodomicp"
-      sensor_name: "scanner_front_left"
-      plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 0.5
-        min_error: 999
-        max_points: 0
-      follow: "parameters/test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorLoopClosureIcp"
-        #  name: "processorloopclosureicp"
-        #  sensor_name: "scanner_front_left"
-        #  plugin: "laser"
-        #  time_tolerance: 0.1
-        #  apply_loss_function: true
-        #  keyframe_vote:
-        #    voting_active: false
-        #  recent_frames_ignored: 3
-        #  frames_ignored_after_loop: 0
-        #  max_error_threshold: 0.02
-        #  min_points_percent: 40
-        #  max_loops: 3
-        #  max_candidates: 5
-        #  max_attempts: 15
-        #  candidate_generation: "random" # 'random' or 'tree'
-        #  icp:
-        #    follow: "parameters/csm.yaml"
-
-      
-  ROS subscriber:
-    -
-      package: "wolf_ros_laser"
-      type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
-      sensor_name: "scanner_front_left"
-      load_params_from_msg: true
-
-  ROS publisher:
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTf"
-      topic: " "
-      period: 0.1
-      follow: "parameters/test_publisher_ros_node.yaml"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherGraph"
-      topic: "graph"
-      period: 1
-      viz_overlapped_factors: true
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTrajectory"
-      topic: "trajectory"
-      period: 0.5
-      frame_id: "map"
-    -
-      package: "wolf_ros_laser"
-      type: "PublisherLaserMap"
-      topic: "map"
-      period: 1
-      map_frame_id: "map"
-      update_dist_th: 0.05
-      update_angle_th: 0.05
-      max_n_cells: 1000000
-      grid_size: 0.1
-      p_free: 0.3
-      p_obst: 0.8
-      p_free_th: 0.2
-      p_obst_th: 0.9
-      discard_max_range: true

yaml/trajectory_analysys/G-1_0.yaml → yaml/trajectory_analysys/G.yaml

@@ -41,14 +41,6 @@ config:
       name: "processorodomicp"
       sensor_name: "scanner_front_left"
       plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 1
-        min_error: 999
-        max_points: 0
       follow: "parameters/test_laser_processor.yaml"
         #-
         #  type: "ProcessorLoopClosureIcp"
@@ -75,7 +67,7 @@ config:
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
+      topic: "/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
 

yaml/trajectory_analysys/E-0_5.yaml → yaml/trajectory_analysys/T.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-0_1.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_T.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -12,13 +12,17 @@ config:
     tree_manager:
       type: "none"
     frame_structure: "POV"
-    dimension: 2
+    dimension: 3
     prior:
-      mode: "fix"
+      mode: "factor"
       $state:
-        P: [0,0]
-        O: [0]
-        V: [0,0]
+        P: [0,0,0]
+        O: [0,0,0,1]
+        V: [0,0,0]
+      $sigma:
+        P: [0.01, 0.01, 0.01]
+        O: [1, 1, 1]
+        V: [0.01, 0.01, 0.01]
       time_tolerance: 0.1
     node_rate: 100
   map:
@@ -35,15 +39,19 @@ config:
       name: "scanner_front_left"
       plugin: "laser"
       follow: "parameters/test_laser_params.yaml"
+    # -
+    #   type: "SensorImu"
+    #   name: "bno"
+    #   plugin: "imu"
+    #   extrinsic:
+    #     pose: [0,0,0,0,0,0,1]  
+    #   follow: "parameters/test_imu_params_bno.yaml" 
     -
-      type: "SensorImu2d"
-      name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_params_bno.yaml" 
-    -
-      type: "SensorImu2d"
+      type: "SensorImu"
       name: "microstrain"
       plugin: "imu"
+      extrinsic:
+        pose: [0,0,0,0,0,0,1]  
       follow: "parameters/test_imu_params_microstrain.yaml" 
 
   processors:
@@ -52,43 +60,16 @@ config:
       name: "processorodomicp"
       sensor_name: "scanner_front_left"
       plugin: "laser"
-      keyframe_vote:
-        voting_active: true
-        min_features_for_keyframe: 10
-        min_dist: 999
-        min_angle: 999
-        min_time: 0.1
-        min_error: 999
-        max_points: 0
       follow: "parameters/test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorLoopClosureIcp"
-        #  name: "processorloopclosureicp"
-        #  sensor_name: "scanner_front_left"
-        #  plugin: "laser"
-        #  time_tolerance: 0.1
-        #  apply_loss_function: true
-        #  keyframe_vote:
-        #    voting_active: false
-        #  recent_frames_ignored: 10
-        #  frames_ignored_after_loop: 0
-        #  max_error_threshold: 0.02
-        #  min_points_percent: 40
-        #  max_loops: 1
-        #  max_candidates: 5
-        #  max_attempts: 5
-        #  candidate_generation: "random" # 'random' or 'tree'
-        #  icp:
-        #    follow: "parameters/csm.yaml"
-    -
-      type: "ProcessorImu2d"
-      name: "processorimu2dbno"
-      sensor_name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_processor_bno.yaml"
+    # -
+    #   type: "ProcessorImu"
+    #   name: "processorimubno"
+    #   sensor_name: "bno"
+    #   plugin: "imu"
+    #   follow: "parameters/test_imu_processor_bno.yaml"
     -
-      type: "ProcessorImu2d"
-      name: "processorimu2dmicro"
+      type: "ProcessorImu"
+      name: "processorimumicro"
       sensor_name: "microstrain"
       plugin: "imu"
       follow: "parameters/test_imu_processor_microstrain.yaml"
@@ -97,19 +78,19 @@ config:
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
+      topic: "/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
-    #-
-    #  package: "wolf_ros_imu"
-    #  type: "SubscriberImuEnableable"
-    #  topic: "/ana/sensors/bno055_imu/imumal"
-    #  sensor_name: "bno"
-    #  follow: "parameters/test_imu_subscriber_bno.yaml"
+    # -
+    #   package: "wolf_ros_imu"
+    #   type: "SubscriberImuEnableable"
+    #   topic: "/imu_bnomal"
+    #   sensor_name: "bno"
+    #   follow: "parameters/test_imu_subscriber_bno.yaml"
     -
       package: "wolf_ros_imu"
       type: "SubscriberImuEnableable"
-      topic: "/ana/imu/data"
+      topic: "/imu_micro"
       sensor_name: "microstrain"
       follow: "parameters/test_imu_subscriber_microstrain.yaml"
 
@@ -126,32 +107,32 @@ config:
       topic: "graph"
       period: 1
       viz_overlapped_factors: true
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTrajectory"
-      topic: "trajectory"
-      period: 1
-      frame_id: "map"
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherTrajectory"
+    #   topic: "trajectory"
+    #   period: 1
+    #   frame_id: "map"
     -
       package: "wolf_ros_node"
       type: "PublisherPose"
       topic: "pose"
       extrinsics: false
-      period: 0.005
+      period: 0.01
       frame_id: "map"
-    -
-      package: "wolf_ros_laser"
-      type: "PublisherLaserMap"
-      topic: "map"
-      period: 1
-      map_frame_id: "map"
-      update_dist_th: 0.05
-      update_angle_th: 0.05
-      max_n_cells: 1000000
-      grid_size: 0.1
-      p_free: 0.3
-      p_obst: 0.8
-      p_free_th: 0.2
-      p_obst_th: 0.9
-      discard_max_range: true
+    # -
+    #   package: "wolf_ros_laser"
+    #   type: "PublisherLaserMap"
+    #   topic: "map"
+    #   period: 1
+    #   map_frame_id: "map"
+    #   update_dist_th: 0.05
+    #   update_angle_th: 0.05
+    #   max_n_cells: 1000000
+    #   grid_size: 0.1
+    #   p_free: 0.3
+    #   p_obst: 0.8
+    #   p_free_th: 0.2
+    #   p_obst_th: 0.9
+    #   discard_max_range: true
 

yaml/trajectory_analysys/parameters/test_imu_subscriber_bno.yaml

@@ -7,7 +7,7 @@ cov_source: "sensor"
 in_degrees: true
 
 #all
-topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
+topic_enable: "/imu_bno/enable"
 static_init_duration: 5
 lowpass_filter: true
 lowpass_cutoff_freq: 5

yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml

@@ -7,7 +7,7 @@ cov_source: "sensor"
 in_degrees: false
 
 #all
-topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
+topic_enable: "/imu_micro/enable"
 static_init_duration: 3
 lowpass_filter: false
 lowpass_cutoff_freq: 5

yaml/trajectory_analysys/parameters/test_laser_processor.yaml

+keyframe_vote:
+  voting_active: true
+  min_features_for_keyframe: 10
+  min_dist: 999
+  min_angle: 999
+  min_time: 10
+  min_error: 999
+  max_points: 0
 max_new_features: 15
 icp:
   follow: "parameters/csm.yaml"

yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml

 map_frame_id: "map"
 odom_frame_id: "ana/odom"
 base_frame_id: "ana/base_footprint"
+  #odom_frame_id: "helena/odom"
+  #base_frame_id: "helena/base_footprint"
 publish_odom_tf: false