From afb257900ab33b63976b88fe0bb85144c147d0fe Mon Sep 17 00:00:00 2001
From: Lovro Markovic <lovro.markovic@fer.hr>
Date: Fri, 10 Sep 2021 14:01:47 +0200
Subject: [PATCH] [ICRA2022] Final config

---
 src/filter.cpp        | 2 +-
 src/sensor_client.cpp | 2 +-
 src/sensor_tf.cpp     | 6 +++++-
 src/sensor_tf.h       | 2 +-
 4 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/filter.cpp b/src/filter.cpp
index 393e5fc..20ad4ea 100644
--- a/src/filter.cpp
+++ b/src/filter.cpp
@@ -269,7 +269,7 @@ void EsEkf2::poseMeasurementUpdateDrift(const Matrix3d&             R_cov,
   Matrix<double, N_STATES + 2, N_STATES> H_dx = MatrixXd::Zero(N_STATES + 2, N_STATES);
   // Scola equation:(280)
   H_dx.block<6, 6>(0, 0)    = MatrixXd::Identity(6, 6);
-  H_dx.block<4, 3>(6, 6)    = 0.5 * sf::firstOrderApprox(m_est_quaternion);
+  H_dx.block<4, 3>(6, 6)    = 0.5 * sf::firstOrderApproxLocal(m_est_quaternion);
   H_dx.block<12, 12>(10, 9) = MatrixXd::Identity(12, 12);
   H_dx.block<4, 3>(22, 21)  = 0.5 * sf::firstOrderApproxLocal(est_quaternion_drift);
 
diff --git a/src/sensor_client.cpp b/src/sensor_client.cpp
index 2bcc379..3b3bfd8 100644
--- a/src/sensor_client.cpp
+++ b/src/sensor_client.cpp
@@ -158,7 +158,7 @@ void SensorClient::stateEstimation(const ros::TimerEvent& /* unused */)
     sensor_ptr->publishState(sensor_state);
     sensor_ptr->publishTransformedPose();
     sensor_ptr->publishDrift();
-    m_sensor_tf.publishSensorOrigin(*sensor_ptr);
+    m_sensor_tf.publishSensorOrigin(*sensor_ptr, m_es_ekf.getOrientation());
   }
 
   if (!measurement && !prediction) {
diff --git a/src/sensor_tf.cpp b/src/sensor_tf.cpp
index 646386a..c963f0c 100644
--- a/src/sensor_tf.cpp
+++ b/src/sensor_tf.cpp
@@ -10,7 +10,7 @@ sf::SensorTF::SensorTF(std::string base_link) : m_base_link(std::move(base_link)
   m_base_link_origin = m_base_link + "/robot";
 }
 
-void sf::SensorTF::publishSensorOrigin(const Sensor& sensor)
+void sf::SensorTF::publishSensorOrigin(const Sensor& sensor, const Quaterniond& ekf_orientation)
 {
   Eigen::Vector3d    position;
   Eigen::Quaterniond orientation;
@@ -23,6 +23,10 @@ void sf::SensorTF::publishSensorOrigin(const Sensor& sensor)
     orientation = sensor.getOrientation();
   }
 
+  if (!sensor.isOrientationSensor()) {
+    orientation = ekf_orientation;
+  }
+
   // Get the inverse transformation
   tf2::Transform tf_inv(
     tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()),
diff --git a/src/sensor_tf.h b/src/sensor_tf.h
index 83d25ee..ad0a433 100644
--- a/src/sensor_tf.h
+++ b/src/sensor_tf.h
@@ -21,7 +21,7 @@ class SensorTF
    *
    * @param sensor Ovbject type Sensor.
    */
-  void publishSensorOrigin(const Sensor& sensor);
+  void publishSensorOrigin(const Sensor& sensor, const Quaterniond& ekf_orientation);
 
   /**
    * @brief Publish estimated origin from the odometry message.