From 8c7ac56f8ff2ade06da3c8448fdf4342475f645c Mon Sep 17 00:00:00 2001
From: Praveen Kumar <pravinblaze@hotmail.com>
Date: Wed, 8 Apr 2020 11:25:49 +0530
Subject: [PATCH] Implemented changes to account for corners of unregistered
 vehicles during localization.

---
 .../trafficmanager/LocalizationStage.cpp      | 26 ++++++++++++++-----
 .../trafficmanager/LocalizationUtils.cpp      |  5 ++--
 .../carla/trafficmanager/LocalizationUtils.h  |  3 ++-
 3 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
index 6a65395fe..9aba9f548 100644
--- a/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
+++ b/LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
@@ -607,17 +607,29 @@ namespace LocalizationConstants {
         cg::Location location = it->second->GetLocation();
         const auto type = it->second->GetTypeId();
 
-        SimpleWaypointPtr nearest_waypoint = nullptr;
+        std::vector<SimpleWaypointPtr> nearest_waypoints;
         if (type[0] == 'v') {
-          nearest_waypoint = local_map.GetWaypointInVicinity(location);
+          auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(it->second);
+          cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
+          float bx = extent.x;
+          float by = extent.y;
+          std::vector<cg::Location> corners = {location + cg::Location(bx, by, 0.0f),
+                                               location + cg::Location(-bx, by, 0.0f),
+                                               location + cg::Location(bx, -by, 0.0f),
+                                               location + cg::Location(-bx, -by, 0.0f)};
+          for (cg::Location &vertex: corners) {
+            SimpleWaypointPtr nearest_waypoint = local_map.GetWaypointInVicinity(vertex);
+            if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetPedWaypoint(vertex);};
+            if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);};
+            nearest_waypoints.push_back(nearest_waypoint);
+          }
         } else if (type[0] == 'w') {
-          nearest_waypoint = local_map.GetPedWaypoint(location);
-        }
-        if (nearest_waypoint == nullptr) {
-          nearest_waypoint = local_map.GetWaypoint(location);
+          SimpleWaypointPtr nearest_waypoint = local_map.GetPedWaypoint(location);
+          if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);};
+          nearest_waypoints.push_back(nearest_waypoint);
         }
 
-        track_traffic.UpdateUnregisteredGridPosition(it->first, nearest_waypoint);
+        track_traffic.UpdateUnregisteredGridPosition(it->first, nearest_waypoints);
 
         ++it;
       }
diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
index 8fea56e8e..6cb7dc1a2 100644
--- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
+++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
@@ -62,7 +62,8 @@ namespace LocalizationConstants {
 
   TrackTraffic::TrackTraffic() {}
 
-  void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint) {
+  void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id,
+                                                    const std::vector<SimpleWaypointPtr> waypoints) {
 
     // Add actor entry, if not present.
     if (actor_to_grids.find(actor_id) == actor_to_grids.end()) {
@@ -85,7 +86,7 @@ namespace LocalizationConstants {
     current_grids.clear();
 
     // Step through buffer and update grid list for actor and actor list for grids.
-    if (waypoint != nullptr) {
+    for (auto &waypoint: waypoints) {
 
       GeoGridId ggid = waypoint->GetGeodesicGridId();
       current_grids.insert(ggid);
diff --git a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h
index 40dd00df0..a005b3e2b 100644
--- a/LibCarla/source/carla/trafficmanager/LocalizationUtils.h
+++ b/LibCarla/source/carla/trafficmanager/LocalizationUtils.h
@@ -50,7 +50,8 @@ namespace traffic_manager {
     ActorIdSet GetPassingVehicles(uint64_t waypoint_id);
 
     void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer);
-    void UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint);
+    void UpdateUnregisteredGridPosition(const ActorId actor_id,
+                                        const std::vector<SimpleWaypointPtr> waypoints);
     /// Method to return the wayPoints from the waypoint Buffer by using target point distance
     std::pair<SimpleWaypointPtr,uint64_t> GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance);