localizaiton result add vehicle orientation term

modules/localization/common/localization_gflags.cc

@@ -130,6 +130,19 @@ DEFINE_bool(gnss_only_init, false,
 DEFINE_bool(enable_lidar_localization, true,
             "Enable lidar-based localization.");
 
+// imu vehicle extrinsic
+DEFINE_string(
+    vehicle_imu_file,
+    "modules/localization/msf/params"
+    "/vehicle_params/vehicle_imu_extrinsics.yaml",
+    "Vehicle coord to imu coord.");
+DEFINE_bool(if_vehicle_imu_from_file, true,
+            "Whether load vehicle imu extrinsic from yaml file");
+DEFINE_double(imu_vehicle_qx, 0.0, "Vehicle imu quaternion qx");
+DEFINE_double(imu_vehicle_qy, 0.0, "Vehicle imu quaternion qy");
+DEFINE_double(imu_vehicle_qz, 0.0, "Vehicle imu quaternion qz");
+DEFINE_double(imu_vehicle_qw, 1.0, "Vehicle imu quaternion qw");
+
 // debug
 DEFINE_bool(use_visualize, false, "");
 

modules/localization/common/localization_gflags.h

@@ -98,6 +98,14 @@ DECLARE_bool(imu_coord_rfu);
 DECLARE_bool(gnss_only_init);
 DECLARE_bool(enable_lidar_localization);
 
+// imu vehicle extrinsic
+DECLARE_string(vehicle_imu_file);
+DECLARE_bool(if_vehicle_imu_from_file);
+DECLARE_double(imu_vehicle_qx);
+DECLARE_double(imu_vehicle_qy);
+DECLARE_double(imu_vehicle_qz);
+DECLARE_double(imu_vehicle_qw);
+
 // Debug
 DECLARE_bool(use_visualize);
 

modules/localization/conf/localization.conf

@@ -194,5 +194,30 @@
 # default: 0.0
 --imu_to_ant_offset_uz=0.08
 
+# Whether load vehicle coord to imu coord quaternion from yaml file
+# type: bool
+# default: true
+--if_vehicle_imu_from_file=true
+
+# vehicle coord to imu coord quaternion qx
+# type: double
+# default: 0.0
+--imu_vehicle_qx=0.0
+
+# vehicle coord to imu coord quaternion qy
+# type: double
+# default: 0.0
+--imu_vehicle_qy=0.0
+
+# vehicle coord to imu coord quaternion qz
+# type: double
+# default: 0.0
+--imu_vehicle_qz=0.0
+
+# vehicle coord to imu coord quaternion qw
+# type: double
+# default: 1.0
+--imu_vehicle_qw=1.0
+
 # The tem folder for localziation visualziation
 --map_visual_dir=data/map_visual

modules/localization/msf/msf_localization.cc

@@ -167,16 +167,47 @@ void MSFLocalization::InitParams() {
   AERROR << "lidar_height: " << localizaiton_param_.lidar_height_file;
 
   localizaiton_param_.utm_zone_id = FLAGS_local_utm_zone_id;
-  // try load zone id from config file in local_map folder
+  // try load zone id from local_map folder
   if (FLAGS_if_utm_zone_id_from_folder) {
-    bool success = LoadZoneIdFromFile(localizaiton_param_.map_path,
+    bool success = LoadZoneIdFromFolder(localizaiton_param_.map_path,
                                       &localizaiton_param_.utm_zone_id);
     if (!success) {
-      AWARN << "Can't load utm zone id from config file, use default value.";
+      AWARN << "Can't load utm zone id from map folder, use default value.";
     }
   }
   AINFO << "utm zone id: " << localizaiton_param_.utm_zone_id;
 
+  // vehicle imu extrinsic
+  localizaiton_param_.imu_vehicle_quat[0] = FLAGS_imu_vehicle_qx;
+  localizaiton_param_.imu_vehicle_quat[1] = FLAGS_imu_vehicle_qy;
+  localizaiton_param_.imu_vehicle_quat[2] = FLAGS_imu_vehicle_qz;
+  localizaiton_param_.imu_vehicle_quat[3] = FLAGS_imu_vehicle_qw;
+  // try to load imu vehicle quat from file
+  if (FLAGS_if_vehicle_imu_from_file) {
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = 0.0;
+    double qw = 0.0;
+
+    std::string vehicle_imu_file =
+        common::util::TranslatePath(FLAGS_vehicle_imu_file);
+    AINFO << "Vehile imu file: " << vehicle_imu_file;
+
+    if (LoadImuVehicleExtrinsic(vehicle_imu_file, &qx, &qy, &qz, &qw)) {
+      localizaiton_param_.imu_vehicle_quat[0] = qx;
+      localizaiton_param_.imu_vehicle_quat[1] = qy;
+      localizaiton_param_.imu_vehicle_quat[2] = qz;
+      localizaiton_param_.imu_vehicle_quat[3] = qw;
+    } else {
+      AWARN << "Can't load imu vehicle quat from file, use default value.";
+    }
+  }
+  AINFO << "imu_vehicle_quat: "
+        << localizaiton_param_.imu_vehicle_quat[0] << " "
+        << localizaiton_param_.imu_vehicle_quat[1] << " "
+        << localizaiton_param_.imu_vehicle_quat[2] << " "
+        << localizaiton_param_.imu_vehicle_quat[3];
+
   // common
   localizaiton_param_.imu_rate = FLAGS_imu_rate;
   localizaiton_param_.enable_lidar_localization =
@@ -389,7 +420,28 @@ bool MSFLocalization::LoadGnssAntennaExtrinsic(
   return false;
 }
 
-bool MSFLocalization::LoadZoneIdFromFile(const std::string &folder_path,
+bool MSFLocalization::LoadImuVehicleExtrinsic(
+    const std::string &file_path, double *quat_qx, double *quat_qy,
+    double *quat_qz, double *quat_qw) {
+  if (!common::util::PathExists(file_path)) {
+    return false;
+  }
+  YAML::Node config = YAML::LoadFile(file_path);
+  if (config["transform"]) {
+    if (config["transform"]["translation"]) {
+      if (config["transform"]["rotation"]) {
+        *quat_qx = config["transform"]["rotation"]["x"].as<double>();
+        *quat_qy = config["transform"]["rotation"]["y"].as<double>();
+        *quat_qz = config["transform"]["rotation"]["z"].as<double>();
+        *quat_qw = config["transform"]["rotation"]["w"].as<double>();
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+bool MSFLocalization::LoadZoneIdFromFolder(const std::string &folder_path,
                                          int *zone_id) {
   std::string map_zone_id_folder;
   if (common::util::DirectoryExists(folder_path + "/map/000/north")) {

modules/localization/msf/msf_localization.h

@@ -87,7 +87,10 @@ class MSFLocalization : public LocalizationBase {
                                 double *offset_y, double *offset_z,
                                 double *uncertainty_x, double *uncertainty_y,
                                 double *uncertainty_z);
-  bool LoadZoneIdFromFile(const std::string &folder_path, int *zone_id);
+  bool LoadImuVehicleExtrinsic(const std::string &file_path,
+                                double *quat_qx, double *quat_qy,
+                                double *quat_qz, double *quat_qw);
+  bool LoadZoneIdFromFolder(const std::string &folder_path, int *zone_id);
 
  private:
   apollo::common::monitor::MonitorLogger monitor_logger_;

modules/localization/msf/params/vehicle_params/vehicle_imu_extrinsics.yaml

+transform:
+  translation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+  rotation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 1.0

modules/localization/msf/params/vehicle_params/vehicle_imu_extrinsics.yaml~

+transform:
+  translation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+  rotation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 1.0

modules/localization/proto/pose.proto

@@ -25,9 +25,9 @@ message Pose {
   // The VRP is the center of rear axle.
   optional apollo.common.PointENU position = 1;
 
-  // A quaternion that represents the rotation from the map coordinate
-  // (East/North/Up) to the
-  // vehicle coordinate (Right/Forward/Up).
+  // A quaternion that represents the rotation from the IMU coordinate
+  // (Right/Forward/Up) to the
+  // world coordinate (East/North/Up).
   optional apollo.common.Quaternion orientation = 2;
 
   // Linear velocity of the VRP in the map reference frame.
@@ -49,7 +49,7 @@ message Pose {
   // Right/forward/up in meters per square second.
   optional apollo.common.Point3D linear_acceleration_vrf = 7;
 
-  // Angular velocity of the vehicle in the vehicle reference frame.
+  // Angular velocity of the VRP in the vehicle reference frame.
   // Around right/forward/up axes in radians per second.
   optional apollo.common.Point3D angular_velocity_vrf = 8;
 
@@ -57,4 +57,9 @@ message Pose {
 
   // Roll/pitch/yaw in radians.
   optional apollo.common.Point3D euler_angles = 9;
+
+  // A quaternion that represents the rotation from the vehicle coordinate
+  // (Right/Forward/Up) to the
+  // world coordinate (East/North/Up).
+  optional apollo.common.Quaternion orientation_vehicle_world = 10;
 }