From 8e216c6da64cd49386eeeb5fd47178fd2a74f76e Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Wed, 24 Jan 2024 15:09:12 +0100
Subject: [PATCH] fixing only initial heading instead of the whole orientation

---
 config/default.yaml     |   2 +-
 src/.transform.cpp.swo  | Bin 12288 -> 0 bytes
 src/VinsRepublisher.cpp |  45 +++++++++++++++++++++++++++++-----------
 3 files changed, 34 insertions(+), 13 deletions(-)
 delete mode 100644 src/.transform.cpp.swo

diff --git a/config/default.yaml b/config/default.yaml
index 9a26b87..848f2c3 100644
--- a/config/default.yaml
+++ b/config/default.yaml
@@ -1,4 +1,4 @@
-# Subtract the first pose from all msgs so that the origin is in zero (OpenVINS sometimes starts rotated by pi/2)
+# Subtract the first heading from all msgs so that the origin is in zero (OpenVINS sometimes starts rotated by pi/2)
 init_in_zero: true
 
 rate_limiter:
diff --git a/src/.transform.cpp.swo b/src/.transform.cpp.swo
deleted file mode 100644
index 4330561f3654cdf4d9546ba17dabdcd7759aefc3..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 12288
zcmeI2-EQ1O6vw9&Aqp)ZxI*G$x)sQ~N^B=lo7Fa5Bt9wv-2`ZW8zeJp&t~1OXRPt;
zrU?x^08fB;04|Vt1#WnPO1(n@E>R`8hAaMKJI*GXP29^!W9h&Cm~)Qj{Ep{XQQY@G
zU+>YjyF{?PLP+q-KVP<=bI1=53Ax8apGSfzGj_^W*Y2Cj(A$yfewMP3dzlKoY$5|a
zSk8~NVj>$zmAGM=o^n$=WC>UT^$CpiV6lCLtSm42#_3w)Dt+zM&+7YIOH05Kummgt
zOTZGa1S|ndz!Et31ax|Ve2XleDKl9t_ZN=s%Tv3v1S|ndz!I<oECEZv60ig;0ZYIV
zummiDb4Y+igj{}}kl!Hl|Nn!(|DU`}$gj}%Py($&H=t*si_oK&2>B7ppdR!Q^dYnk
zIncjX2>BEG19}_kLYJV&m*EFJgsww>zld|_E9iCT(F=rp2W>)cK!05#<Y(w7Xb;+e
zde9x{Kg{_F^m{q~$0i56Spt@TC143y0+zrzC$N8$P^!2t_BGG;tF81ND?$DISZ1A0
zmd3(qR;OEW!WGr|?=321#NAMFrg=W#DC6?pSTP+-;rOoa`%BFQn#xRX9ek;nk0Bz#
zWW5&cD>vgJs)n8Flg7K6B`J@ZH)+c&Y%>k#kmBRVAiq=P^p27&3R$KxxxOkL3ZLWQ
z-0Fg>q=_emyF+hL-(B@rSK95B!`oGs<xEa;-7~IF=d%0sE)U?cXl8h$y?lMSyj`8?
znaq-G+eHO<@gr5t#X;Vtu64qgEa!*Ws~g5-K0nNE-7tp6{4fvB2$SbKDhfZF7eK}N
zGjUc@(_v=ws=}O=)O48Hys9uKClwCkQ4|amOL!1Rpwhs%!HpC1A95CP<xbn;tKv~~
zsx{VIk|J!e*s$n39U*s$c7Ub(>E_*FedG3~(_k5+Zw?v@N2_u;`(h2_VcX_1;acqm
zutcl)7?0%_u^zrFCrrgmguGm8!CI!HPKS*~GGtn+wTX<QZr79@^Jx{0M$0q~JDnXC
z>)TTGc$TqkzE<}ZUB}S!FiF5m#*L=wN$8)Zg>9g*eZWLC;(3oVMDy#hh_%DicDO+C
zJ6WKOWY>I)9rO<yPy=ydp=rX<In6;~9}bynwQ{%S*JytGh*_ie!D?oKmbE;sIvA_f
zDT(2FLVtEbDKdRnc=i4SUE?h?^J9Fs4*GgoxtZyL>a|#eqjALP(c*Y>%kK`mbgtL&
rJHmL%wtP8w6}9Qw%$Rz2V!@f}F+KD?8RI>sgn1zxiHOR3kh%CjiCot|

diff --git a/src/VinsRepublisher.cpp b/src/VinsRepublisher.cpp
index 78827ac..90e09e8 100644
--- a/src/VinsRepublisher.cpp
+++ b/src/VinsRepublisher.cpp
@@ -53,9 +53,8 @@ class VinsRepublisher : public nodelet::Nodelet {
   double      _rate_limiter_rate_;
   bool        _init_in_zero_;
 
-  bool               got_init_pose_ = false;
-  Eigen::Vector3d    init_position_eigen_;
-  Eigen::Quaterniond init_orientation_eigen_;
+  bool   got_init_pose_ = false;
+  double init_hdg_;
 
   bool validateOdometry(const nav_msgs::Odometry &odometry);
 
@@ -68,6 +67,8 @@ class VinsRepublisher : public nodelet::Nodelet {
 
   /* transformation handler */
   mrs_lib::Transformer transformer_;
+
+  geometry_msgs::Point rotatePointByHdg(const geometry_msgs::Point &pt_in, const double hdg_in) const;
 };
 
 //}
@@ -378,17 +379,19 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
   /* //} */
 
   // save initial pose to subtract it from all messages to compensate initialized orientation ambiguity
-  if (_init_in_zero_ && !got_init_pose_) {
-    init_position_eigen_    = mrs_lib::geometry::toEigen(vins_pose_mrs_world.pose.position);
-    init_orientation_eigen_ = mrs_lib::geometry::toEigen(vins_pose_mrs_world.pose.orientation).inverse();
-    got_init_pose_          = true;
+  if (_init_in_zero_) {
+    if (!got_init_pose_) {
+      init_hdg_ = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading();
+      ROS_INFO("[VinsRepublisher]: init hdg: %.2f", init_hdg_);
+      got_init_pose_ = true;
+    }
+
+    // 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_);
   }
 
-  // compensate initialized orientation ambiguity
-  vins_pose_mrs_world.pose.position =
-      mrs_lib::geometry::fromEigen(init_orientation_eigen_ * (mrs_lib::geometry::toEigen(vins_pose_mrs_world.pose.position) - init_position_eigen_));
-  vins_pose_mrs_world.pose.orientation =
-      mrs_lib::geometry::fromEigen(mrs_lib::geometry::toEigen(vins_pose_mrs_world.pose.orientation) * init_orientation_eigen_);
 
   // fill the new transformed odom message
   nav_msgs::Odometry odom_trans;
@@ -417,6 +420,24 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
 
 //}
 
+/* //{ rotatePointByHdg() */
+geometry_msgs::Point VinsRepublisher::rotatePointByHdg(const geometry_msgs::Point &pt_in, const double hdg_in) const {
+
+  const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+
+  const tf2::Vector3 vec_tf2(pt_in.x, pt_in.y, pt_in.z);
+
+  const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+
+  geometry_msgs::Point pt_out;
+  pt_out.x = vec_rotated.x();
+  pt_out.y = vec_rotated.y();
+  pt_out.z = vec_rotated.z();
+
+  return pt_out;
+}
+//}
+
 }  // namespace vins_republisher
 /* every nodelet must export its class as nodelet plugin */
 PLUGINLIB_EXPORT_CLASS(vins_republisher::VinsRepublisher, nodelet::Nodelet);