From fa7617e325836cb223a4bfef5f71195d52d61872 Mon Sep 17 00:00:00 2001
From: zhangkailin <kailin.zhang@calmcar.com>
Date: Thu, 8 Sep 2022 17:14:12 +0800
Subject: [PATCH 1/2] fix lanes result

---
 PINetTensorrt.cpp | 43 +++++++++++++++++++++++++++----------------
 1 file changed, 27 insertions(+), 16 deletions(-)

diff --git a/PINetTensorrt.cpp b/PINetTensorrt.cpp
index 1af81aa..311aaf3 100644
--- a/PINetTensorrt.cpp
+++ b/PINetTensorrt.cpp
@@ -417,14 +417,19 @@ LaneLines PINetTensorrt::generateLaneLine(float* confidance_data, float* offsets
     LaneLines laneLines;
     std::vector<cv::Vec4f> laneFeatures;
 
-    auto findNearestFeature = [&laneFeatures](const cv::Vec4f& feature) -> int {
+
+    auto findNearestFeature = [&laneFeatures](const cv::Vec4f& feature) -> std::pair<int, float> {
+        int index=-1;
+        float min_feature_dis = 10000.;
         for (int i = 0; i < laneFeatures.size(); ++i) {
             auto delta = laneFeatures[i] - feature;
-            if (delta.dot(delta) <= threshold_instance) {
-                return i;
+            auto alpha = pow(delta[0],2)+pow(delta[1],2)+pow(delta[2],2)+pow(delta[3],2);
+            if (sqrt(alpha) <= min_feature_dis){
+                index = i;
+                min_feature_dis = sqrt(alpha);
             }
         }
-        return -1;
+        return std::pair<int, float>(index, min_feature_dis);
     };
 
     for (int i = 0; i < dim.d[1]; ++i) {
@@ -439,20 +444,26 @@ LaneLines PINetTensorrt::generateLaneLine(float* confidance_data, float* offsets
             if (point.y > dim.d[1] || point.y < 0.f) continue;
 
             const cv::Vec4f& feature = features.at<cv::Vec4f>(i, j);
-            int lane_index = findNearestFeature(feature);
-            
-            if (lane_index == -1) {
+            std::pair<int, float> lane_index = findNearestFeature(feature); 
+
+            if (lane_index.first == -1) {
+                laneLines.emplace_back(LaneLine({point}));
+                laneFeatures.emplace_back(feature);
+            } 
+            else if (lane_index.second <= threshold_instance ) {
+
+                auto& laneline = laneLines[lane_index.first ];
+                auto& lanefeature = laneFeatures[lane_index.first ];
+                if (lane_index.second <= threshold_instance ){                    
+                    auto point_size = laneline.size(); 
+                    lanefeature = lanefeature.mul(cv::Vec4f::all(point_size)) + feature;
+                    lanefeature = lanefeature.mul(cv::Vec4f::all(1.f / (point_size + 1)));
+                    laneline.emplace_back(point);
+                }
+            }
+            else{
                 laneLines.emplace_back(LaneLine({point}));
                 laneFeatures.emplace_back(feature);
-            } else {
-                auto& laneline = laneLines[lane_index];
-                auto& lanefeature = laneFeatures[lane_index];
-
-                auto point_size = laneline.size(); 
-
-                lanefeature = lanefeature.mul(cv::Vec4f::all(point_size)) + feature;
-                lanefeature = lanefeature.mul(cv::Vec4f::all(1.f / (point_size + 1)));
-                laneline.emplace_back(point);
             }
         }
     }

From 4c8445ea7bb2495014dc30a048cd1b1831d9d1dd Mon Sep 17 00:00:00 2001
From: kailin zhang <10251153+zkailinzhang@users.noreply.github.com>
Date: Tue, 1 Nov 2022 14:49:21 +0800
Subject: [PATCH 2/2] [bug] offset xy convert

---
 PINetTensorrt.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/PINetTensorrt.cpp b/PINetTensorrt.cpp
index 311aaf3..e6f6f43 100644
--- a/PINetTensorrt.cpp
+++ b/PINetTensorrt.cpp
@@ -389,7 +389,7 @@ void PINetTensorrt::generatePostData(float* confidance_data, float* offsets_data
             for (int j = 0; j < dim.d[2]; ++j) {
                 if ((int)mask.at<uchar>(i, j)) {
                     cv::Vec2f pointOffset = offsets.at<cv::Vec2f>(i, j);
-                    cv::Point2f point(pointOffset[1] + j, pointOffset[0] + i);
+                    cv::Point2f point(pointOffset[0] + j, pointOffset[1] + i);
                     cv::circle(offsetImage, point * 8, 3, color, -1);
                 }
             }
@@ -439,7 +439,7 @@ LaneLines PINetTensorrt::generateLaneLine(float* confidance_data, float* offsets
             }
 
             const cv::Vec2f& offset = offsets.at<cv::Vec2f>(i, j);
-            cv::Point2f point(offset[1] + j, offset[0] + i);
+            cv::Point2f point(offset[0] + j, offset[1] + i);
             if (point.x > dim.d[2] || point.x < 0.f) continue;
             if (point.y > dim.d[1] || point.y < 0.f) continue;