changed evaluate tool according to document

docs/howto/how_to_run_MSF_localization_module_on_your_local_computer.md

@@ -13,6 +13,11 @@ cp DATA_PATH/params/ant_imu_leverarm.yaml /apollo/modules/localization/msf/param
 cp DATA_PATH/params/velodyne64_novatel_extrinsics_example.yaml /apollo/modules/localization/msf/params/velodyne_params/
 cp DATA_PATH/params/velodyne64_height.yaml /apollo/modules/localization/msf/params/velodyne_params/
 ```
+The meaning of each file
+ - ant_imu_leverarm.yaml:  Lever arm value
+ - velodyne64_novatel_extrinsics_example.yaml: Transform from Imu coord to Lidar coord
+ - velodyne64_height.yaml: Height of the Lidar relative to the ground
+
 ### 2.2 Configure Map Path
 Add config of map path in /apollo/modules/localization/conf/localization.conf
 ```
@@ -77,10 +82,11 @@ If you record localization result in step 5, you will also need to end the recor
 ```
 
 ## 7. Verify the localization result (optional)
-Assume the folder stored recording bag in step 5 is OUTPUT_PATH.
+
 ```
-./scripts/msf_local_evaluation.sh OUTPUT_PATH
+./scripts/msf_local_evaluation.sh OUTPUT_PATH ANT_IMU_PATH
 ```
+OUTPUT_PATH is the folder stored recording bag in step 5, and ANT_IMU_PATH is the file stored lever arm value.
 
 This script compares the localization results of MSF mode to RTK mode.
 
@@ -90,7 +96,7 @@ And we can get the statistical results like this
 
 ![2](images/msf_localization/localization_result.png)
 
-The first table is the statistical data of SINS. The output frequency of SINS is 200hz. The second table is the statistc result of Lidar localization. The output frequency of Lidar localization is 5hz.
+The first table is the statistical data of Fusion localization. The second table is the statistical result of Lidar localization. The third table is the statistical result of GNSS localization.
 
 The meaning of each row in the table
  - error：  the plane error, unit is meter

docs/howto/how_to_run_MSF_localization_module_on_your_local_computer_cn.md

@@ -20,6 +20,10 @@
   cp DATA_PATH/params/velodyne64_novatel_extrinsics_example.yaml /apollo/modules/localization/msf/params/velodyne_params/
   cp DATA_PATH/params/velodyne64_height.yaml /apollo/modules/localization/msf/params/velodyne_params/
 ```
+各个外参的意义
+ - ant_imu_leverarm.yaml： 杆臂值参数，GNSS天线相对Imu的距离
+ - velodyne64_novatel_extrinsics_example.yaml： Lidar相对Imu的外参
+ - velodyne64_height.yaml： Lidar相对地面的高度
 
 ### 2.2 配置地图路径
 在/apollo/modules/localization/conf/localization.conf中添加关于地图路径的配置
@@ -61,6 +65,9 @@ rosbag play *.bag
 该脚本会在后台运行录包程序，并将存放路径输出到终端上。
 
 ### 可视化定位结果
+
+运行可视化工具
+
 ```
 ./scripts/localization_online_visualizer.sh
 ```
@@ -76,6 +83,15 @@ rosbag play *.bag
 可视化效果如下
 ![1](images/msf_localization/online_visualizer.png)
 
+如果发现可视化工具运行时卡顿，可使用如下命令重新编译可视化工具
+
+```
+cd /apollo
+bazel build -c opt //modules/localization/msf/local_tool/local_visualization/online_visual:online_local_visualizer
+```
+
+编译选项-c opt优化程序性能，从而使可视化工具可以实时运行。
+
 ## 6. 结束运行定位模块
 
 ```
@@ -90,11 +106,11 @@ rosbag play *.bag
 
 ## 7. 验证定位结果（可选）
 
-假设步骤5中录取的数据存放路径为OUTPUT_PATH
+假设步骤5中录取的数据存放路径为OUTPUT_PATH，杆臂值外参的路径为ANT_IMU_PATH
 
 运行脚本
 ```
-./scripts/msf_local_evaluation.sh OUTPUT_PATH
+./scripts/msf_local_evaluation.sh OUTPUT_PATH ANT_IMU_PATH
 ```
 该脚本会以RTK定位模式为基准，将多传感器融合模式的定位结果进行对比。
 
@@ -104,7 +120,7 @@ rosbag play *.bag
 
 ![2](images/msf_localization/localization_result.png)
 
-可以看到两组统计结果，第一组是组合导航(输出频率200hz)的统计结果，第二组是点云定位(输出频率5hz)的统计结果。
+可以看到两组统计结果，第一组是组合导航(输出频率200hz)的统计结果，第二组是点云定位(输出频率5hz)的统计结果，第三组是GNSS定位(输出频率约1hz)的统计结果。
 
 表格中各项的意义， 
  - error：  平面误差，单位为米

modules/localization/msf/local_tool/data_extraction/compare_poses.cc

@@ -14,17 +14,38 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include <boost/filesystem.hpp>
-#include <boost/program_options.hpp>
 #include <fstream>
 #include <map>
 #include <vector>
 
+#include "boost/filesystem.hpp"
+#include "boost/program_options.hpp"
+#include "yaml-cpp/yaml.h"
+
 #include "modules/common/log.h"
 #include "modules/common/math/quaternion.h"
 #include "modules/localization/msf/common/io/velodyne_utility.h"
 #include "modules/localization/msf/local_tool/local_visualization/offline_visual/offline_local_visualizer.h"
 
+static bool LoadGnssAntennaExtrinsic(
+    const std::string &file_path, Eigen::Vector3d *imu_ant_offset) {
+  CHECK_NOTNULL(imu_ant_offset);
+
+  YAML::Node config = YAML::LoadFile(file_path);
+  if (config["leverarm"]) {
+    if (config["leverarm"]["primary"]["offset"]) {
+      (*imu_ant_offset)[0] =
+          config["leverarm"]["primary"]["offset"]["x"].as<double>();
+      (*imu_ant_offset)[1] =
+          config["leverarm"]["primary"]["offset"]["y"].as<double>();
+      (*imu_ant_offset)[2] =
+          config["leverarm"]["primary"]["offset"]["z"].as<double>();
+    }
+    return true;
+  }
+  return false;
+}
+
 int main(int argc, char **argv) {
   boost::program_options::options_description boost_desc("Allowed options");
   boost_desc.add_options()("help", "produce help message")(
@@ -39,7 +60,10 @@ int main(int argc, char **argv) {
       "provide lidar localization file.")(
       "compare_file",
       boost::program_options::value<std::string>(),
-      "provide compare file.");
+      "provide compare file.")(
+      "imu_to_ant_offset_file",
+      boost::program_options::value<std::string>()->default_value(""),
+      "provide imu to ant offset file.");
 
   boost::program_options::variables_map boost_args;
   boost::program_options::store(
@@ -62,6 +86,17 @@ int main(int argc, char **argv) {
       boost_args["loc_file_b"].as<std::string>();
   const std::string compare_file = in_folder + "/" +
       boost_args["compare_file"].as<std::string>();
+  const std::string imu_to_ant_offset_file =
+      boost_args["imu_to_ant_offset_file"].as<std::string>();
+
+  Eigen::Vector3d imu_ant_offset = Eigen::Vector3d::Zero();
+  if (imu_to_ant_offset_file != "") {
+    bool suc = LoadGnssAntennaExtrinsic(
+        imu_to_ant_offset_file, &imu_ant_offset);
+    if (suc == false) {
+      return 0;
+    }
+  }
 
   std::vector<Eigen::Affine3d> poses_a;
   std::vector<Eigen::Vector3d> stds_a;
@@ -132,6 +167,11 @@ int main(int argc, char **argv) {
         double pitch_b = euler_b.pitch();
         double yaw_b = euler_b.yaw();
 
+        Eigen::Vector3d offset = quatd_b * imu_ant_offset;
+        transd_a.x() = transd_a.x() - offset[0];
+        transd_a.y() = transd_a.y() - offset[1];
+        transd_a.z() = transd_a.z() - offset[2];
+
         double x_diff = fabs(transd_a.x() - transd_b.x());
         double y_diff = fabs(transd_a.y() - transd_b.y());
         double z_diff = fabs(transd_a.z() - transd_b.z());

modules/tools/localization/evaluate_compare.py

@@ -131,7 +131,7 @@ def get_angle_stat_from_data(data):
     return stat
 
 
-def parse_file(filename):
+def parse_file(filename, type):
     file = open(filename, "r")
     lines = file.readlines()
     print "%d frames" % len(lines)
@@ -158,7 +158,18 @@ def parse_file(filename):
             error.append(math.sqrt(x * x + y * y))
             #print "%f %f %f" % (error[-1], error_lon[-1], error_lat[-1])
     file.close()
+    if type == "all":
+        print_distance_error(error, error_lon, error_lat, error_alt)
+        print_angle_error(error_roll, error_pitch, error_yaw)
+    elif type == "distance_only":
+        print_distance_error(error, error_lon, error_lat, error_alt)
+    elif type == "angle_only":
+        print_angle_error(error_roll, error_pitch, error_yaw)
+    else:
+        print_distance_error(error, error_lon, error_lat, error_alt)
+        print_angle_error(error_roll, error_pitch, error_yaw)
     
+def print_distance_error(error, error_lon, error_lat, error_alt):
     print "criteria : mean     std      max      < 30cm   < 20cm   < 10cm  con_frames(>30cm)"
     result = get_stat_from_data(error)
     res = get_stat2_from_data(error)
@@ -177,6 +188,7 @@ def parse_file(filename):
     print "error alt: %06f %06f %06f %06f %06f %06f %06d" % \
     (result[0], result[1], result[2], result[3], result[4], result[5], res[2]) 
 
+def print_angle_error(error_roll, error_pitch, error_yaw):
     print "criteria : mean     std      max      < 1.0d   < 0.6d   < 0.3d  con_frames(>1.0d)"
     result = get_angle_stat_from_data(error_roll)
     res = get_angle_stat2_from_data(error_roll)
@@ -194,9 +206,10 @@ def parse_file(filename):
 
 if __name__ == '__main__':
     if len(sys.argv) < 2:
-        print "python evaluation.py [evaluation file]"
+        print "python evaluation.py [evaluation file] [evaluation type]"
     elif not os.path.isfile(sys.argv[1]):
         print "file not exist"
+    elif len(sys.argv) < 3:
+        parse_file(sys.argv[1], "all")
     else:
-        parse_file(sys.argv[1])
-
+        parse_file(sys.argv[1], sys.argv[2])
\ No newline at end of file

scripts/msf_local_evaluation.sh

 #! /bin/bash
 if [ $# -lt 1 ]; then
-    echo "Usage: msf_monitor_data_exporter.sh [bags folder]"
+    echo "Usage:"
+    echo "$0 [bags folder]                   evaluate fusion and lidar localization result"
+    echo "$0 [bags folder] [ant arm file]    evaluate fusion, lidar and gnss localization result"
     exit 1;
 fi
 
+IN_FOLDER=$1
+if [ $# -eq 2 ]; then
+  ANT_IMU_FILE=$2
+else
+  ANT_IMU_FILE=""
+fi
+
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "${DIR}/.."
 
@@ -20,8 +29,6 @@ LIDAR_LOC_FILE="lidar_loc.txt"
 FUSION_LOC_FILE="fusion_loc.txt"
 ODOMETRY_LOC_FILE="odometry_loc.txt"
 
-IN_FOLDER=$1
-
 function data_exporter() {
   local BAG_FILE=$1
   local OUT_FOLDER=$2
@@ -41,6 +48,7 @@ function compare_poses() {
     --in_folder $IN_FOLDER \
     --loc_file_a $GNSS_LOC_FILE \
     --loc_file_b $ODOMETRY_LOC_FILE \
+    --imu_to_ant_offset_file "$ANT_IMU_FILE" \
     --compare_file "compare_gnss_odometry.txt"
 
   $APOLLO_BIN_PREFIX/modules/localization/msf/local_tool/data_extraction/compare_poses \
@@ -60,29 +68,46 @@ cd $IN_FOLDER
 for item in $(ls -l *.bag | awk '{print $9}')
 do
   DIR_NAME=$(echo $item | cut -d . -f 1)
-  mkdir $DIR_NAME
+  if [ -d "${DIR_NAME}" ]; then
+    rm -r ${DIR_NAME}
+  fi
+  mkdir ${DIR_NAME}
   data_exporter "${item}" "${DIR_NAME}"
   compare_poses "${DIR_NAME}/pcd"
 done
 
+rm -rf compare_fusion_odometry_all.txt
 touch compare_fusion_odometry_all.txt
 for item in  $(find . -name "compare_fusion_odometry.txt")
 do 
   cat $item >> compare_fusion_odometry_all.txt
 done
 
+rm -rf compare_lidar_odometry_all.txt
 touch compare_lidar_odometry_all.txt
 for item in  $(find . -name "compare_lidar_odometry.txt")
 do 
   cat $item >> compare_lidar_odometry_all.txt
 done
 
+rm -rf compare_gnss_odometry_all.txt
 touch compare_gnss_odometry_all.txt
 for item in  $(find . -name "compare_gnss_odometry.txt")
 do 
   cat $item >> compare_gnss_odometry_all.txt
 done
 
+echo ""
+echo "Fusion localization result:"
 python ${APOLLO_ROOT_DIR}/modules/tools/localization/evaluate_compare.py compare_fusion_odometry_all.txt
+
+echo ""
+echo "Lidar localization result:"
 python ${APOLLO_ROOT_DIR}/modules/tools/localization/evaluate_compare.py compare_lidar_odometry_all.txt
-# python ${APOLLO_ROOT_DIR}/modules/tools/localization/evaluate_compare.py compare_gnss_odometry_all.txt
+
+if [ $# -eq 2 ]; then
+  echo ""
+  echo "GNSS localization result:"
+  python ${APOLLO_ROOT_DIR}/modules/tools/localization/evaluate_compare.py\
+    compare_gnss_odometry_all.txt distance_only
+fi