yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

opt road match

Browse Source
This commit is contained in:
tongchenfei
2021-03-16 16:54:10 +08:00
parent c942144189
commit a6a2c2a39b
5 changed files with 116 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/map-custom/build.gradle
View File

@@ -67,7 +67,7 @@ dependencies {
implementation project(':foudations:mogo-commons')
}
implementation 'com.zhidaoauto.machine:map:1.0.0-vr-8.3.4'
implementation 'com.zhidaoauto.machine:map:1.0.0-vr-8.3.5'
// implementation 'com.zhidaoauto.machine:map:1.0.0-vr-test-3.4'
}

127
libraries/map-custom/src/main/java/com/mogo/map/impl/custom/AMapViewWrapper.java
View File

@@ -68,13 +68,10 @@ import org.jetbrains.annotations.Nullable;
import org.json.JSONObject;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Random;
import static java.lang.Math.PI;
public class AMapViewWrapper implements IMogoMapView,
IMogoMapUIController,
LocationListener,
@@ -1045,71 +1042,83 @@ public class AMapViewWrapper implements IMogoMapView,
return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK, true);
}
int[] colors = new int[]{Color.RED, Color.GREEN, Color.BLUE};
int colorIndex = 0;
private double[] matchRoad(String id, double lon, double lat, double angle, boolean isGpsLocation, boolean isRTK, boolean usdCache) {
double wgs[] = new double[]{lon, lat};
long start = System.currentTimeMillis();
//
// try {
// PolylineOptions options = new PolylineOptions( );
// options.setColor( Color.WHITE );
// options.setId( "test-line"+new Random( ).nextLong() );
// options.setGps( true );
// options.setLineWidth( 3 );
// List<LonLatPoint> points = singlePointRoadInfo.getCoords();
// ArrayList< LonLat > lonLats = new ArrayList<>( );
// for ( LonLatPoint point : points ) {
// lonLats.add( new LonLat( point.getLongitude(), point.getLatitude() ) );
// }
// options.setLonLats( lonLats );
// mMapView.getMapAutoViewHelper().drawThickLine( options );
// } catch( Exception e ){
// e.printStackTrace();
// }
SinglePointRoadInfo singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(((float) wgs[0]), ((float) wgs[1]), ((float) angle), isGpsLocation, isRTK);
if ( singlePointRoadInfo == null || singlePointRoadInfo.getCoords() == null || singlePointRoadInfo.getCoords().isEmpty() ) {
return null;
}
double matchedPoint[] = PointInterpolatorUtil.mergeToRoad(wgs[0], wgs[1], singlePointRoadInfo.getCoords());
return matchedPoint;
// RoadCacheWrapper roadCache = roadCacheMap.get(id);
// if (roadCache == null) {
// SinglePointRoadInfo singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(((float) wgs[0]), ((float) wgs[1]), ((float) angle), isGpsLocation, isRTK);
// if (singlePointRoadInfo != null && singlePointRoadInfo.getCoords() != null && !singlePointRoadInfo.getCoords().isEmpty()) {
// Log.i("timer-matchRoad-4", "cost " + (System.currentTimeMillis() - start) + "ms roadId: " + singlePointRoadInfo.getRoadId());
// roadCache = new RoadCacheWrapper(singlePointRoadInfo.getCoords());
// }
// SinglePointRoadInfo singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(((float) wgs[0]), ((float) wgs[1]), ((float) angle), isGpsLocation, isRTK);
// if ( singlePointRoadInfo == null || singlePointRoadInfo.getCoords() == null || singlePointRoadInfo.getCoords().isEmpty() ) {
// return null;
// }
//
// Log.i("timer-matchRoad-1", "cost " + (System.currentTimeMillis() - start) + "ms");
// if (roadCache != null
// && roadCache.getRoad() != null
// && !roadCache.getRoad().isEmpty()) {
// start = System.currentTimeMillis();
// double matchedPoint[] = PointInterpolatorUtil.mergeToRoad(wgs[0], wgs[1], roadCache.getRoad());
// if (matchedPoint[2] > 0 && matchedPoint[2] < 1.5) {
//// if (roadCache.inCache(matchedPoint[0], matchedPoint[1])) {
// roadCacheMap.put(id, roadCache);
// Log.i("timer-matchRoad-3", "cost " + (System.currentTimeMillis() - start) + "ms");
// return matchedPoint;
//// }
//// roadCacheMap.put(id, null);
//// Log.i("timer-matchRoad-2", "cost " + (System.currentTimeMillis() - start) + "ms roadId: "+roadCache.getLastLat());
//// return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK,false);
// }else{
// double matchedPoint[] = PointInterpolatorUtil.mergeToRoad(wgs[0], wgs[1], singlePointRoadInfo.getCoords());
// return matchedPoint;
RoadCacheWrapper roadCache = roadCacheMap.get(id);
double matchThreshold = -1;
if (roadCache == null) {
SinglePointRoadInfo singlePointRoadInfo = MapDataApi.INSTANCE.getSinglePointMatchRoad(((float) wgs[0]), ((float) wgs[1]), ((float) angle), isGpsLocation, isRTK);
if (singlePointRoadInfo != null && singlePointRoadInfo.getCoords() != null && !singlePointRoadInfo.getCoords().isEmpty()) {
Log.i("timer-matchRoad-4", "cost " + (System.currentTimeMillis() - start) + "ms roadId: " + singlePointRoadInfo.getRoadId());
roadCache = new RoadCacheWrapper(singlePointRoadInfo.getCoords());
roadCache.setLaneWidth(singlePointRoadInfo.getLaneWidth());
// try {
// PolylineOptions options = new PolylineOptions( );
// options.setColor( colors[colorIndex++] );
// options.setId( "test-line"+new Random( ).nextLong() );
// options.setGps( true );
// options.setLineWidth( 3 );
// List<LonLatPoint> points = singlePointRoadInfo.getCoords();
// ArrayList< LonLat > lonLats = new ArrayList<>( );
// for ( LonLatPoint point : points ) {
// lonLats.add( new LonLat( point.getLongitude(), point.getLatitude() ) );
// }
// options.setLonLats( lonLats );
// mMapView.getMapAutoViewHelper().drawThickLine( options );
// if (colorIndex >= colors.length) {
// colorIndex = 0;
// }
// } catch( Exception e ){
// e.printStackTrace();
// }
}
}
Log.i("timer-matchRoad-1", "cost " + (System.currentTimeMillis() - start) + "ms");
if (roadCache != null
&& roadCache.getRoad() != null
&& !roadCache.getRoad().isEmpty()) {
start = System.currentTimeMillis();
matchThreshold = roadCache.getLaneWidth() / 4;
double matchedPoint[] = PointInterpolatorUtil.mergeToRoad(wgs[0], wgs[1], roadCache.getRoad());
if (matchThreshold>0&&matchedPoint[2] > 0 && matchedPoint[2] <= matchThreshold) {
// if (roadCache.inCache(matchedPoint[0], matchedPoint[1])) {
roadCacheMap.put(id, roadCache);
Log.i("timer-matchRoad-3", "cost " + (System.currentTimeMillis() - start) + "ms");
return matchedPoint;
// }
// roadCacheMap.put(id, null);
// Log.i("timer-matchRoad-2", "cost " + (System.currentTimeMillis() - start) + "ms roadId: " + roadCache.getLastLat());
//// if(usdCache) {
//// return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK, false);
//// }else{
//// return null;
//// }
// Log.i("timer-matchRoad-2", "cost " + (System.currentTimeMillis() - start) + "ms roadId: "+roadCache.getLastLat());
// return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK,false);
} else if (matchedPoint[2] > matchThreshold && matchedPoint[2] < 1.5) {
return null;
} else {
roadCacheMap.put(id, null);
Log.i("timer-matchRoad-2", "cost " + (System.currentTimeMillis() - start) + "ms roadId: " + roadCache.getLastLat());
if (usdCache) {
return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK, false);
} else {
return null;
}
// return null;
// }
// }
// roadCacheMap.put(id, null);
// return null;
}
}
roadCacheMap.put(id, null);
return null;
}
@Override

10
libraries/map-custom/src/main/java/com/mogo/map/impl/custom/RoadCacheWrapper.java
View File

@@ -14,6 +14,8 @@ public class RoadCacheWrapper {
private double lastDistanceDiff;
private int roadLength = -1;
private float laneWidth = -1;
public RoadCacheWrapper(List<LonLatPoint> road) {
setRoad(road);
}
@@ -51,6 +53,14 @@ public class RoadCacheWrapper {
return 0;
}
public float getLaneWidth() {
return laneWidth;
}
public void setLaneWidth(float laneWidth) {
this.laneWidth = laneWidth;
}
public boolean inCache(double lon, double lat) {
if (roadLength > 0) {
LonLatPoint start = road.get(0);

12
libraries/map-custom/src/main/java/com/mogo/map/impl/custom/utils/PointInterpolatorUtil.java
View File

@@ -1,10 +1,13 @@
package com.mogo.map.impl.custom.utils;
import android.util.Log;
import com.mogo.cloud.commons.utils.CoordinateUtils;
import com.mogo.map.MogoLatLng;
import com.mogo.utils.logger.Logger;
import com.zhidaoauto.map.sdk.open.query.LonLatPoint;
import java.text.NumberFormat;
import java.util.ArrayList;
import java.util.List;
@@ -134,18 +137,25 @@ public class PointInterpolatorUtil {
if (cross < 0) {
// 垂足没有在线段内，所以也无需计算最短距离
// result[2] = Math.sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
// Log.d("getFootAndMinDistance", "垂足不在线段内--1");
return result;
}
double d2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
if (cross > d2) {
// 垂足没有在线段内，所以也无需计算最短距离
// result[2] = Math.sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2));
// Log.d("getFootAndMinDistance", "垂足不在线段内--2");
return result;
}
double r = cross / d2;
result[0] = x1 + (x2 - x1) * r;
result[1] = y1 + (y2 - y1) * r;
result[2] = Math.sqrt((x - result[0]) * (x - result[0]) + (result[1] - y) * (result[1] - y));
// NumberFormat numberFormat = NumberFormat.getInstance();
// numberFormat.setMaximumFractionDigits(4);
// String formatNum = numberFormat.format(Math.sqrt((x - result[0]) * (x - result[0]) + (result[1] - y) * (result[1] - y)));
// result[2] = Double.parseDouble(formatNum);
result[2] = CoordinateUtils.calculateLineDistance(result[0], result[1], x, y);
// Log.d("getFootAndMinDistance", "计算距离--lon: "+result[0]+" lat: "+result[1]+" dis: "+result[2]);
return result;
}

33
modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/AdasRecognizedResultDrawer.java
View File

@@ -171,7 +171,7 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
mAdasRecognizedMarkersCaches = newAdasRecognizedMarkersCaches;
}
// private final Map< String, Boolean > mIsMatchStatusCache = new ArrayMap<>();
private final Map< String, Boolean > mIsMatchStatusCache = new ArrayMap<>();
private String markerRes;
private String markerRes2;
@@ -204,14 +204,25 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
);
ADASRecognizedResult lastPosition = mLastPositions.remove( uniqueKey );
boolean match = false;
if ( matchedPoint != null ) {
Logger.d( TAG, "matchPoint %s distance = %s",lineCounter, matchedPoint[2] );
boolean match = matchedPoint[2] < 0.45 && matchedPoint[2] > 0;
match = matchedPoint[2] < 1 && matchedPoint[2] > 0;
if ( lastPosition != null ) {
double clearAngle;
if (recognizedListResult.heading > 0 && recognizedListResult.heading <= 90) {
clearAngle = recognizedListResult.heading;
} else if (recognizedListResult.heading > 90 && recognizedListResult.heading <= 180) {
clearAngle = 180 - recognizedListResult.heading ;
} else if (recognizedListResult.heading > 180 && recognizedListResult.heading <= 270) {
clearAngle = recognizedListResult.heading - 180;
}else{
clearAngle = 360 - recognizedListResult.heading;
}
double _angle = Math.atan2(Math.abs(matchedPoint[0] - lastPosition.lon), Math.abs(matchedPoint[1] - lastPosition.lat)) * (180 / PI);
Logger.d( TAG, "matchPoint %s angel = %s", lineCounter,_angle );
_angle = Math.abs(clearAngle - _angle);
Logger.d(TAG, "matchPoint %s angel = %s", lineCounter, _angle);
if ( _angle > 22.5 ) {
match = false;
}
@@ -222,12 +233,12 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
recognizedListResult.lat = matchedPoint[1];
}
}
mLastPositions.put( uniqueKey, recognizedListResult );
// Boolean isMatch = mIsMatchStatusCache.get( uniqueKey );
// if ( matchedPoint != null ) {
// if ( ( isMatch == null || !isMatch ) ) {
// if ( matchedPoint[2] < 0.5 ) {
// if ( matchedPoint[2] < 0.5 && matchedPoint[2] > 0 ) {
// isMatch = true;
// }
// } else {
@@ -238,6 +249,13 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
// if ( isMatch == null ) {
// isMatch = false;
// }
// if (lastPosition != null) {
// double _angle = Math.atan2(Math.abs(matchedPoint[0] - lastPosition.lon), Math.abs(matchedPoint[1] - lastPosition.lat)) * (180 / PI);
// Logger.d(TAG, "matchPoint %s angel = %s", lineCounter, _angle);
// if (_angle > 22.5) {
// isMatch = false;
// }
// }
// mIsMatchStatusCache.put( uniqueKey, isMatch );
//
// if ( isMatch ) {
@@ -245,8 +263,8 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
// recognizedListResult.lat = matchedPoint[1];
// }
// }
mLastPositions.put( uniqueKey, recognizedListResult );
Logger.d( "matchRoad", "cost = %s", System.currentTimeMillis() - start );
IMogoMarker marker = mAdasRecognizedMarkersCaches.remove( uniqueKey );
if ( marker == null || marker.isDestroyed() ) {
@@ -331,6 +349,7 @@ class AdasRecognizedResultDrawer extends BaseDrawer {
SpeedData obj = new SpeedData();
obj.context = mContext;
obj.marker = marker;
// obj.speed = match ? 1.0 : 0.0;
obj.speed = recognizedListResult.speed;//;lineCounter++
msg.obj = obj;
msg.what = MSG_DISPLAY_SPEED;