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

添加道路吸附相关方法注释

Browse Source
This commit is contained in:
tongchenfei
2021-03-30 11:22:53 +08:00
parent 2e4297b5ef
commit d37a77602b
2 changed files with 93 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1044,7 +1044,7 @@ public class AMapViewWrapper implements IMogoMapView,
}
Map<String, RoadCacheWrapper> roadCacheMap = new ConcurrentHashMap<>();
private RoadCacheWrapper noCache = new RoadCacheWrapper(null);
private final RoadCacheWrapper noCache = new RoadCacheWrapper(null);
@Override
public void clearRoadCacheById(String id) {
@@ -1056,21 +1056,32 @@ 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;
/**
* 带缓存的道路匹配算法
*
* 使用{@link MapDataApi#INSTANCE#getSinglePointMatchRoad()}这个方法获取道路数据会存在一定耗时，目前是4-10ms，因为请求频繁，为了
* 减小这部分耗时，使用一个ConcurrentHashMap{@link #roadCacheMap}缓存道路数据，此缓存是以目标车id为key，道路数据为value，以此减少
* 道路数据获取次数。
*
* 当出现道路改变的情况，需要重新获取道路数据，采用递归的方式重获道路数据以及添加缓存，为了避免一直获取不到道路数据，一直递归，增加useCache参数跳出递归。
*
* 道路改变的情况主要分为：
* 1. 目标车距离道路中心线距离超过阈值 matchThreshold（取路宽的1/4为阈值）；
* 2. 目标车到道路中心线的映射点不在道路上（此判断在{@link PointInterpolatorUtil#mergeToRoad(double, double, List)}），也就是说已经驶出了这条道路
*
* @param id 目标车唯一标识
* @param lon 目标车经度
* @param lat 目标车纬度
* @param angle 目标车角度
* @param isGpsLocation true-使用gps定位数据
* @param isRTK true-使用rtk数据
* @param usdCache true-使用道路缓存 false-不使用道路缓存
* @return double[]{匹配后经度，匹配后纬度，目标车距离车道中心线距离}，若匹配后经纬度值为-1，则没有匹配成功
*/
private double[] matchRoad(String id, double lon, double lat, double angle, boolean isGpsLocation, boolean isRTK, boolean usdCache) {
double wgs[] = new double[]{lon, lat};
double[] wgs = new double[]{lon, lat};
long start = System.currentTimeMillis();
//
// 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);
double matchThreshold = -1;
if (roadCache == null||roadCache == noCache) {
@@ -1079,6 +1090,7 @@ public class AMapViewWrapper implements IMogoMapView,
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++] );
@@ -1105,22 +1117,22 @@ public class AMapViewWrapper implements IMogoMapView,
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());
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());
// return matchRoad(id, lon, lat, angle, isGpsLocation, isRTK,false);
} else if (matchedPoint[2] > matchThreshold && matchedPoint[2] < 1.5) {
// 目标车在阈值范围外，也就是说距离道路中心线太远了，就不吸附了
return null;
} else {
// 目标车到道路中心线的映射点不在道路上，也就是已经使出了缓存的那条道路，需要重新获取一条道路，用useCache这个参数来避免重复递归
roadCacheMap.put(id, noCache);
Log.i("timer-matchRoad-2", "cost " + (System.currentTimeMillis() - start) + "ms roadId: " + roadCache.getLastLat());
if (usdCache) {
@@ -1128,7 +1140,6 @@ public class AMapViewWrapper implements IMogoMapView,
} else {
return null;
}
// return null;
}
}
roadCacheMap.put(id, noCache);

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

@@ -1,14 +1,10 @@
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;
/**
@@ -35,22 +31,22 @@ public class PointInterpolatorUtil {
* @deprecated 这个方法有问题，并不能算出来想要的值
*/
@Deprecated
public static void interpolate( List< MogoLatLng > points ) {
if ( points.size() >= 2 ) {
public static void interpolate(List<MogoLatLng> points) {
if (points.size() >= 2) {
// 插值
for ( int i = 0; i < points.size() - 1; i++ ) {
MogoLatLng current = points.get( i );
MogoLatLng next = points.get( i + 1 );
float distance = CoordinateUtils.calculateLineDistance( current.lon, current.lat, next.lon, next.lat );
Logger.d( TAG, i + ": " + distance );
if ( distance > DISTANCE_THRESHOLD ) {
int inter = ( int ) ( distance / DISTANCE_THRESHOLD ) + 1;
for ( int j = 1; j < inter; j++ ) {
double newLat = current.lat + ( next.lat - current.lat ) * j / inter;
double newLon = current.lon + ( next.lon - current.lon ) * j / inter;
Logger.d( TAG, "distance: " + distance + ", j: " + j + ", nextLat: " + next.lat + ", nextLon: " + next.lon + ", newLat: " + newLat + ", newLon: " + newLon );
points.add( i + 1, new MogoLatLng( newLat, newLon ) );
current = points.get( ++i );
for (int i = 0; i < points.size() - 1; i++) {
MogoLatLng current = points.get(i);
MogoLatLng next = points.get(i + 1);
float distance = CoordinateUtils.calculateLineDistance(current.lon, current.lat, next.lon, next.lat);
Logger.d(TAG, i + ": " + distance);
if (distance > DISTANCE_THRESHOLD) {
int inter = (int) (distance / DISTANCE_THRESHOLD) + 1;
for (int j = 1; j < inter; j++) {
double newLat = current.lat + (next.lat - current.lat) * j / inter;
double newLon = current.lon + (next.lon - current.lon) * j / inter;
Logger.d(TAG, "distance: " + distance + ", j: " + j + ", nextLat: " + next.lat + ", nextLon: " + next.lon + ", newLat: " + newLat + ", newLon: " + newLon);
points.add(i + 1, new MogoLatLng(newLat, newLon));
current = points.get(++i);
}
}
}
@@ -58,77 +54,67 @@ public class PointInterpolatorUtil {
}
}
public static double[] mergeToRoad( double lon, double lat, List< LonLatPoint > road ) {
/**
* 道路吸附算法
* <p>
* 所谓的道路数据，实际就是道路对应的点集，每两个点之间是直线，但是两点间距并不固定，点集内点的数量也不固定，点集是有序的，按道路方向排序，road[0]是起点。
* 为了避免拐弯道路的问题，先使用{@link #getCloseTwoPoint(int, int, double, double, List)}从道路数据里面找出距离目标点最近的两个点，记为A、B，最近的两个点就在目标点一前一后排列，
* 这样的话，求一下目标点到AB的垂直映射以及距离{@link #getFootAndMinDistance(double, double, double, double, double, double)}，就是吸附后的经纬度和距离
*
* @param lon 目标经度
* @param lat 目标纬度
* @param road 目标道路数据
* @return double[]{吸附后的经度，吸附后的纬度，目标经纬度距离道路的垂直距离}
*/
public static double[] mergeToRoad(double lon, double lat, List<LonLatPoint> road) {
int closeStart = 0;
int closeEnd = road.size() - 1;
int[] result = getCloseTwoPoint(closeStart,closeEnd, lon, lat, road );
LonLatPoint start = road.get( result[0] );
LonLatPoint end = road.get( result[1] );
// Logger.d( TAG, "mergeToRoad start: " + closeStart + " end: " + closeEnd );
// return getMid(start, end);
// double[] foot = getFoot( lon, lat, start, end );
// float d = CoordinateUtils.calculateLineDistance( foot[0], foot[1], lon, lat );
// Logger.d( TAG, "distance to mid line==" + d );
// return new double[]{foot[0], foot[1], d};
int[] result = getCloseTwoPoint(closeStart, closeEnd, lon, lat, road);
LonLatPoint start = road.get(result[0]);
LonLatPoint end = road.get(result[1]);
return getFootAndMinDistance(lon, lat, start.getLongitude(), start.getLatitude(), end.getLongitude(), end.getLatitude());
}
private static int[] getCloseTwoPoint(int closeStart,int closeEnd, double lon, double lat, List< LonLatPoint > road ) {
if ( closeEnd - closeStart == 1 ) {
return new int[]{closeStart,closeEnd};
/**
* 获取距离目标点经纬度最近的道路点index
* <p>
* 采用二分查找思想，先用道路数据的起点和终点分别计算距离目标点的距离：
* 若起点距离较远，则说明目标点在整条道路的后半段，则将起点后移至原起点和终点的中间，继续递归计算；
* 同理，若终点距离较远，则目标点在前半段，则终点前移，继续递归计算；
* 递归结束条件是起点index和终点index间隔是1；
*
* @param closeStart 距离目标点最近的起始点index
* @param closeEnd 距离目标点最近的终点index
* @param lon 目标点经度
* @param lat 目标点纬度
* @param road 目标道路
* @return int[]{距离目标点最近的起始点index,距离目标点最近的终点index}
*/
private static int[] getCloseTwoPoint(int closeStart, int closeEnd, double lon, double lat, List<LonLatPoint> road) {
if (closeEnd - closeStart == 1) {
return new int[]{closeStart, closeEnd};
}
LonLatPoint start = road.get( closeStart );
LonLatPoint end = road.get( closeEnd );
float startDistance = CoordinateUtils.calculateLineDistance( start.getLongitude(), start.getLatitude(), lon, lat );
float endDistance = CoordinateUtils.calculateLineDistance( end.getLongitude(), end.getLatitude(), lon, lat );
if ( startDistance > endDistance ) {
closeStart += ( closeEnd - closeStart ) / 2;
LonLatPoint start = road.get(closeStart);
LonLatPoint end = road.get(closeEnd);
float startDistance = CoordinateUtils.calculateLineDistance(start.getLongitude(), start.getLatitude(), lon, lat);
float endDistance = CoordinateUtils.calculateLineDistance(end.getLongitude(), end.getLatitude(), lon, lat);
if (startDistance > endDistance) {
closeStart += (closeEnd - closeStart) / 2;
} else {
closeEnd -= ( closeEnd - closeStart ) / 2;
closeEnd -= (closeEnd - closeStart) / 2;
}
return getCloseTwoPoint(closeStart,closeEnd, lon, lat, road );
return getCloseTwoPoint(closeStart, closeEnd, lon, lat, road);
}
private static double[] getFoot( double lon, double lat, LonLatPoint beginPt, LonLatPoint endPt ) {
double dx = beginPt.getLatitude() - endPt.getLatitude();
double dy = beginPt.getLongitude() - endPt.getLongitude();
double u = ( lat - beginPt.getLatitude() ) * ( beginPt.getLatitude() - endPt.getLatitude() ) +
( lon - beginPt.getLongitude() ) * ( beginPt.getLongitude() - endPt.getLongitude() );
u = u / ( dx * dx + dy * dy );
return new double[]{beginPt.getLongitude() + u * dy, beginPt.getLatitude() + u * dx};
}
// un minDistance(x: Double, y: Double, x1: Double, y1: Double, x2: Double, y2: Double): Double {
// val cross = (x2 - x1) * (x - x1) + (y2 - y1) * (y - y1)
// println("1.cross:$cross")
// if (cross <= 0) {
// return Math.sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1))
// }
// val d2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)
// println("2.d2:$cross")
// if (cross > d2) {
// return Math.sqrt((x - x2) * (x - x2) + (y - y2) * (y - y2))
// }
// val r = cross / d2
// val px = x1 + (x2 - x1) * r
// val py = y1 + (y2 - y1) * r
// println("3.r:$r,px:$px,py:$py")
// return Math.sqrt((x - px) * (x - px) + (py - y) * (py - y))
// }
/**
* 计算垂足以及最短距离
*
* @param x target point lon
* @param y target point lat
* @param x1 start point lon
* @param y1 start point lat
* @param x2 end point lon
* @param y2 end point lat
* @param x target point lon
* @param y target point lat
* @param x1 线段起点 lon
* @param y1 起点起点 lat
* @param x2 线段终点 lon
* @param y2 线段终点 lat
* @return double[]{footLon,footLat,minDistance} if(footLon == -1) => no foot or foot not in line
*/
private static double[] getFootAndMinDistance(double x, double y, double x1, double y1, double x2, double y2) {
@@ -136,30 +122,17 @@ public class PointInterpolatorUtil {
double cross = (x2 - x1) * (x - x1) + (y2 - y1) * (y - y1);
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;
// 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;
}
private static double[] getMid( LonLatPoint start, LonLatPoint end ) {
return new double[]{( start.getLongitude() + end.getLongitude() ) / 2, ( start.getLatitude() + end.getLatitude() ) / 2};
}
}