chore: move position correction code to lib

and make some improvements in the code.
tidewise · Jan 23, 2025 · ae7baa6 · ae7baa6
1 parent 87d41c1
commit ae7baa6
Show file tree

Hide file tree

Showing 2 changed files with 109 additions and 0 deletions.
diff --git a/src/AIS.cpp b/src/AIS.cpp
@@ -129,3 +129,90 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes
     info.destination = message.get_destination();
     return info;
 }
+
+std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> vesselToWorldOrientation(
+    const std::optional<base::Angle>& yaw,
+    const std::optional<base::Angle>& course_over_ground,
+    double speed_over_ground)
+{
+    const double min_speed_threshold = 0.2;
+
+    if (yaw.has_value()) {
+        return {Eigen::Quaterniond(
+                    Eigen::AngleAxisd(yaw.value().getRad(), Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING};
+    }
+    else if (course_over_ground.has_value() && speed_over_ground >= min_speed_threshold) {
+        return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.value().getRad(),
+                    Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE};
+    }
+    else {
+        return {Eigen::Quaterniond::Identity(),
+            ais_base::PositionCorrectionStatus::POSITION_RAW};
+    }
+}
+
+base::Vector3d sensorToVesselInWorldPose(base::Vector3d sensor2vessel_pos,
+    Eigen::Quaterniond vessel2world_ori)
+{
+    base::Vector3d sensor2vessel_in_world_pos;
+    sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_pos;
+
+    return sensor2vessel_in_world_pos;
+}
+
+base::samples::RigidBodyState convertGPSToUTM(
+    const std::optional<ais_base::Position>& position,
+    gps_base::UTMConverter utm_converter)
+{
+    gps_base::Solution sensor2world_solution;
+    sensor2world_solution.latitude = position.value().latitude.getDeg();
+    sensor2world_solution.longitude = position.value().longitude.getDeg();
+
+    base::samples::RigidBodyState sensor2world_UTM;
+    sensor2world_UTM.position =
+        utm_converter.convertToUTM(sensor2world_solution).position;
+
+    return sensor2world_UTM;
+}
+
+std::pair<base::Angle, base::Angle> convertUTMToGPSInWorldFrame(
+    base::samples::RigidBodyState sensor2world_UTM,
+    base::Vector3d sensor2vessel_in_world_pos,
+    gps_base::UTMConverter utm_converter)
+{
+    base::samples::RigidBodyState vessel2world_UTM;
+    vessel2world_UTM.position = sensor2world_UTM.position + sensor2vessel_in_world_pos;
+
+    gps_base::Solution vessel2world_GPS;
+    vessel2world_GPS = utm_converter.convertUTMToGPS(vessel2world_UTM);
+
+    return {base::Angle::fromDeg(vessel2world_GPS.latitude),
+        base::Angle::fromDeg(vessel2world_GPS.longitude)};
+}
+
+ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position,
+    base::Vector3d const& vessel_reference_position,
+    gps_base::UTMConverter utm_converter)
+{
+    ais_base::Position corrected_position;
+
+    auto [vessel2world_ori, status] = vesselToWorldOrientation(position.yaw,
+        position.course_over_ground,
+        position.speed_over_ground);
+
+    auto sensor2vessel_in_world_pos =
+        sensorToVesselInWorldPose(vessel_reference_position, vessel2world_ori);
+
+    auto [latitude, longitude] =
+        convertUTMToGPSInWorldFrame(convertGPSToUTM(position, utm_converter),
+            sensor2vessel_in_world_pos,
+            utm_converter);
+
+    corrected_position.latitude = latitude;
+    corrected_position.longitude = longitude;
+    corrected_position.correction_status = status;
+
+    return corrected_position;
+}
diff --git a/src/AIS.hpp b/src/AIS.hpp
@@ -10,6 +10,11 @@
 #include <marnav/ais/message_01.hpp>
 #include <marnav/ais/message_05.hpp>
 
+#include <base/samples/RigidBodyState.hpp>
+#include <gps_base/BaseTypes.hpp>
+#include <gps_base/UTMConverter.hpp>
+#include <optional>
+
 namespace nmea0183 {
     class AIS {
         uint32_t mDiscardedSentenceCount = 0;
@@ -44,6 +49,23 @@ namespace nmea0183 {
          */
         uint32_t getDiscardedSentenceCount() const;
 
+        /**
+         * Applies position correction using the vessel reference position and the sensor
+         * offset
+         *
+         * @param position the AIS position to be corrected
+         * @param vessel_reference_position The position of the vessel’s reference point
+         * relative to the sensor
+         * @param utm_converter The UTM converter
+         *
+         * @return The corrected AIS position with updated latitude, longitude, and
+         * correction status
+         */
+        static ais_base::Position applyPositionCorrection(
+            ais_base::Position const& position,
+            base::Vector3d const& vessel_reference_position,
+            gps_base::UTMConverter utm_converter);
+
         static ais_base::Position getPosition(marnav::ais::message_01 const& message);
         static ais_base::VesselInformation getVesselInformation(
             marnav::ais::message_05 const& message);