From 57cbbe7042732819bce3ead471c89fa61563f590 Mon Sep 17 00:00:00 2001
From: zhongchao <zhongchao@zhidaoauto.com>
Date: Wed, 27 Apr 2022 11:20:46 +0800
Subject: [PATCH] teting

---
 .../core/function/map/IdentifyDataDrawer.java | 31 ++++++++++---------
 gradle.properties                             |  2 +-
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
index 1a85cbab03..6e0fdd2ad5 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
@@ -36,6 +36,9 @@ public class IdentifyDataDrawer {
      */
     private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
 
+    /**
+     * kalman缓存数据
+     */
     private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
 
     /**
@@ -108,7 +111,7 @@ public class IdentifyDataDrawer {
             MogoMarkerManager.getInstance(mContext)
                     .updateBatchMarkerPosition(filterList);
         }
-        CallerLogger.INSTANCE.d(M_HMI + "arrow48",  "mMarkersCaches : " + mMarkersCaches.size());
+        CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
     }
 
     /**
@@ -130,16 +133,11 @@ public class IdentifyDataDrawer {
             MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
             if (cacheData != null) {
                 MessagePad.TrackedObject correctData;
-                //todo 进行修正
-                double correctHeading = kalmanCorrect(data);
-                if(correctHeading != cacheData.getHeading()){
-                    CallerLogger.INSTANCE.d(M_HMI + "arrow48", "uuid: " + uuid + " , kalmanCorrect : " + correctHeading + " , 修正 上一帧 : " + cacheData.getHeading() + " , 当前帧 : " + data.getHeading());
-                }
-                correctData = data.toBuilder().setHeading(correctHeading).build();
+                correctData = kalmanCorrectData(data);
                 mFilterTrafficData.add(correctData);
                 //更新已存在的感知物体数据
                 mMarkersCaches.put(uuid, correctData);
-            }else{
+            } else {
                 mMarkersCaches.put(uuid, data);
             }
             trafficDataUuidList.add(uuid);
@@ -147,7 +145,8 @@ public class IdentifyDataDrawer {
         return mFilterTrafficData;
     }
 
-    private double kalmanCorrect(MessagePad.TrackedObject data) {
+    //todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回
+    private MessagePad.TrackedObject kalmanCorrectData(MessagePad.TrackedObject data) {
         String uuid = "" + data.getUuid();
         if (algoCache.containsKey(uuid)) {
             Object o = algoCache.get(uuid);
@@ -159,18 +158,19 @@ public class IdentifyDataDrawer {
             assert cacheTrackObj != null;
             if (data.getSpeed() >= 1.5) {
                 double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]);
-                CallerLogger.INSTANCE.d(M_HMI + "arrow48", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , correct heading : " + heading + "  ---- " + (data.getHeading() - heading));
-                return heading;
+                double correct = heading - data.getHeading() > 30 ? heading : data.getHeading();//todo test
+                CallerLogger.INSTANCE.d(M_HMI + "arrow48", " uuid : " + uuid + " , origin heading : " + data.getHeading() + " , cal heading : " + heading + ", correct : " + correct + " , heading - data.getHeading() > 30 : " + (heading - data.getHeading() > 30));
+                return data.toBuilder().setHeading(heading).setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
             } else {
-                return data.getHeading();
+                return data;
             }
         } else {
-            double r = 0.00005;
+            double r = 0.000005;
             if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
-                r = 0.0001;
+                r = 0.00001;
             }
             algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
-            return data.getHeading();
+            return data;
         }
     }
 
@@ -185,3 +185,4 @@ public class IdentifyDataDrawer {
         trafficDataUuidList.clear();
     }
 }
+
diff --git a/gradle.properties b/gradle.properties
index 239495d895..260008de0f 100644
--- a/gradle.properties
+++ b/gradle.properties
@@ -82,7 +82,7 @@ MOGO_LOCATION_VERSION=1.3.55
 MOGO_TELEMATIC_VERSION=1.3.55
 ######## MogoAiCloudSDK Version ########
 # 自研地图
-MAP_SDK_VERSION=2.1.0.6
+MAP_SDK_VERSION=2.1.0.6-1
 MAP_SDK_OPERATION_VERSION=1.0.12
 # websocket
 WEBSOCKET_VERSION=1.1.7