Add position and velocity covariance matrix for MUST sensor detection…

… data
usdot-fhwa-OPS · Aug 28, 2024 · 13908f0 · 13908f0
1 parent 1615eb5
commit 13908f0
Show file tree

Hide file tree

Showing 6 changed files with 55 additions and 5 deletions.
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json
@@ -37,6 +37,17 @@
             "key":"ProjectionString",
             "default":"+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs",
             "description":"Projection string for projecting cartesian detection data into WSG84 coordinates."
+        },
+        {
+            "key":"DetectionPositionVariance",
+            "default":"0.0625",
+            "description":"Variance of the reported positon data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS."
+        },
+        {
+            "key":"DetectionVelocityVariance",
+            "default":"0.0625",
+            "description":"Variance of the reported velocity data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS."
         }
+
     ]
 }
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp
@@ -29,7 +29,7 @@ namespace MUSTSensorDriverPlugin {
         return detection;
     }
 
-    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString) {
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance) {
         tmx::messages::SensorDetectedObject detectedObject;
         detectedObject.set_objectId(detection.trackID);
         tmx::messages::Position pos(detection.position_x, detection.position_y, 0);
@@ -38,8 +38,21 @@ namespace MUSTSensorDriverPlugin {
         detectedObject.set_timestamp(static_cast<long>(detection.timestamp*1000)); // convert decimal seconds to int milliseconds.
         detectedObject.set_velocity(headingSpeedToVelocity(detection.heading, detection.speed));
         detectedObject.set_type(detectionClassificationToSensorDetectedObjectType(detection.cl));
+        std::vector<std::vector< tmx::messages::Covariance>> positionCov(3, std::vector<tmx::messages::Covariance>(3,tmx::messages::Covariance(0.0) ));
+        // Set X and Y position variance in covariance matrix
+        positionCov[0][0] = tmx::messages::Covariance(positionVariance);
+        positionCov[1][1] = tmx::messages::Covariance(positionVariance);
+        detectedObject.set_positionCovariance(positionCov);
+
+        // Set X and Y Velocity variance in covariance matrix
+        std::vector<std::vector< tmx::messages::Covariance>> velocityCov(3, std::vector<tmx::messages::Covariance>(3,tmx::messages::Covariance(0.0) ));
+        velocityCov[0][0] = tmx::messages::Covariance(velocityVariance);
+        velocityCov[1][1] = tmx::messages::Covariance(velocityVariance);
+        detectedObject.set_velocityCovariance(velocityCov);
+
         detectedObject.set_sensorId(std::string(sensorId));
         detectedObject.set_projString(std::string(projString));
+
         return detectedObject;
     }
     DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept {

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h
@@ -116,7 +116,7 @@ namespace MUSTSensorDriverPlugin {
      * @param projString std::string describing reference point and WGS84 projection of detection information
      * @return tmx::messages::SensorDetectedObject
      */
-    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString);
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance);
 
     /**
      * @brief Function to convert MUSTSensor provided heading and speed to a velocity vector 

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp
@@ -49,6 +49,8 @@ namespace MUSTSensorDriverPlugin {
 			unsigned int port;
 			GetConfigValue<std::string>("DetectionReceiverIP", ip_address);
 			GetConfigValue<uint>("DetectionReceiverPort", port);
+			GetConfigValue<double>("DetectionPositionVariance", positionVariance);
+			GetConfigValue<double>("DetectionVelocityVariance", velocityVariance);
 			createUdpServer(ip_address, port);
 			SetStatus(keyMUSTSensorConnectionStatus, "IDLE");
 
@@ -69,7 +71,7 @@ namespace MUSTSensorDriverPlugin {
 					connected = true;
 					SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED");
 				}
-				tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString);
+				tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString,positionVariance, velocityVariance);
 				PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
 				this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
 			}

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h
@@ -48,6 +48,10 @@ namespace MUSTSensorDriverPlugin
 
             std::string projString;
 
+            double positionVariance = 0.0;
+
+            double velocityVariance = 0.0;
+
             bool connected = false;
 
             // Message receiver thread id

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp
@@ -62,8 +62,8 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
     detection.timestamp = 1719506355.4;
     detection.trackID = 324;
     detection.speed = 5;
-
-    auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String");
+    // 0.0625 variance corresponds to 0.25 std. Assuming Normal distribution and a 95% confidence interval corresponds to +/- 0.5m or m/s respectively. 
+    auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String", 0.0625, 0.0625);
 
     EXPECT_EQ(detection.trackID, sensorDetectedObject.get_objectId());
     EXPECT_DOUBLE_EQ(detection.confidence/100.0, sensorDetectedObject.get_confidence());
@@ -75,6 +75,26 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
     EXPECT_EQ(1719506355400, sensorDetectedObject.get_timestamp());
     EXPECT_EQ("MUSTSensor1", sensorDetectedObject.get_sensorId());
     EXPECT_EQ("PROJ String", sensorDetectedObject.get_projString());
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[0][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][0].value);
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[1][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][2].value);
+
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[0][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][0].value);
+    EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[1][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][2].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][0].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][1].value);
+    EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][2].value);
+
 }
 
 TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType ) {