From 13908f0ce178a6bae5ea6e1a5d4b2af11531ac75 Mon Sep 17 00:00:00 2001 From: dev Date: Wed, 28 Aug 2024 17:02:51 -0400 Subject: [PATCH] Add position and velocity covariance matrix for MUST sensor detection data --- .../MUSTSensorDriverPlugin/manifest.json | 11 +++++++++ .../src/MUSTSensorDetection.cpp | 15 +++++++++++- .../src/MUSTSensorDetection.h | 2 +- .../src/MUSTSensorDriverPlugin.cpp | 4 +++- .../src/MUSTSensorDriverPlugin.h | 4 ++++ .../test/TestMUSTSensorDetection.cpp | 24 +++++++++++++++++-- 6 files changed, 55 insertions(+), 5 deletions(-) diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json index dcd15c60f..31348cb46 100755 --- 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." } + ] } \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index 0256bb93a..0c36fad1a 100644 --- 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(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> positionCov(3, std::vector(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> velocityCov(3, std::vector(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 index 9140b0252..96de21088 100644 --- 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 index 024430b4c..6aa623330 100644 --- 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("DetectionReceiverIP", ip_address); GetConfigValue("DetectionReceiverPort", port); + GetConfigValue("DetectionPositionVariance", positionVariance); + GetConfigValue("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(msg, _name, 0 , IvpMsgFlags_None); } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h index 3d9e33d8c..af3f2893b 100644 --- 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 index d065d31a3..a72a29609 100644 --- 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 ) {