covariance scaling factor for individual element

sbgisen · Nov 16, 2022 · 65f87d7 · 65f87d7
1 parent 5c1361b
commit 65f87d7
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 16 deletions.
diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp
@@ -436,26 +436,18 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
     odom.header.stamp = stamp;
     odom.header.frame_id = "map";
 
-    double fitness_score_coefficient = private_nh.param<double>("odom_covariance_coefficient", 1.0);
     tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast<double>()), odom.pose.pose);
-    for (int i = 0; i < 36; i++) {
-      if (i % 7 == 0) {
-        odom.pose.covariance[i] = fitness_score_coefficient * fitness_score;
-      } else {
-        odom.pose.covariance[i] = 0.0;
-      }
-    }
+    odom.pose.covariance[0] = private_nh.param<double>("cov_scaling_factor_x", 1.0) * fitness_score;
+    odom.pose.covariance[7] = private_nh.param<double>("cov_scaling_factor_y", 1.0) * fitness_score;
+    odom.pose.covariance[14] = private_nh.param<double>("cov_scaling_factor_z", 1.0) * fitness_score;
+    odom.pose.covariance[21] = private_nh.param<double>("cov_scaling_factor_R", 1.0) * fitness_score;
+    odom.pose.covariance[28] = private_nh.param<double>("cov_scaling_factor_P", 1.0) * fitness_score;
+    odom.pose.covariance[35] = private_nh.param<double>("cov_scaling_factor_Y", 1.0) * fitness_score;
+
     odom.child_frame_id = odom_child_frame_id;
     odom.twist.twist.linear.x = 0.0;
     odom.twist.twist.linear.y = 0.0;
     odom.twist.twist.angular.z = 0.0;
-    for (int i = 0; i < 36; i++) {
-      if (i % 7 == 0) {
-        odom.twist.covariance[i] = fitness_score_coefficient * fitness_score;
-      } else {
-        odom.twist.covariance[i] = 0.0;
-      }
-    }
 
     pose_pub.publish(odom);
   }

diff --git a/launch/hdl_localization.launch b/launch/hdl_localization.launch
@@ -62,7 +62,12 @@
       <param name="init_ori_x" value="0.0" />
       <param name="init_ori_y" value="0.0" />
       <param name="init_ori_z" value="0.0" />
-      <param name="odom_covariance_coefficient" value="1.0" />
+      <param name="cov_scaling_factor_x" value="1.0" />
+      <param name="cov_scaling_factor_y" value="1.0" />
+      <param name="cov_scaling_factor_z" value="1.0" />
+      <param name="cov_scaling_factor_R" value="1.0" />
+      <param name="cov_scaling_factor_P" value="1.0" />
+      <param name="cov_scaling_factor_Y" value="1.0" />
 
       <param name="use_global_localization" value="$(arg use_global_localization)" />
     </node>