fixed bad rotation of vins position by initial heading

ctu-mrs · Apr 12, 2024 · 65c019d · 65c019d
1 parent 8e216c6
commit 65c019d
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/VinsRepublisher.cpp b/src/VinsRepublisher.cpp
@@ -389,7 +389,7 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
     // compensate initial heading ambiguity
     const double hdg                     = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading();
     vins_pose_mrs_world.pose.orientation = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).setHeading(hdg - init_hdg_);
-    vins_pose_mrs_world.pose.position    = rotatePointByHdg(vins_pose_mrs_world.pose.position, init_hdg_);
+    vins_pose_mrs_world.pose.position    = rotatePointByHdg(vins_pose_mrs_world.pose.position, -init_hdg_);
   }