和客户同步1.0版本

puzhibing

2026-04-10 ed838e62602c0a63a64ff4bd144a0d16cc9a7cf8

 xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/TrajectoryDensifier.java

@@ -1,5 +1,6 @@
package com.xiaopeng.pushMessage.utils;

import com.xiaopeng.api.dto.operation.LonLatDto;
import org.gavaghan.geodesy.Ellipsoid;
import org.gavaghan.geodesy.GeodeticCalculator;
import org.gavaghan.geodesy.GeodeticCurve;
@@ -7,9 +8,71 @@

import java.time.Instant;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.ThreadPoolExecutor;
import java.util.concurrent.TimeUnit;

public class TrajectoryDensifier {

    private static final int THREAD_NUM = 5;


    public static List<LonLatDto> densify(List<LonLatDto> originalPoints, int distance) {
        int num = originalPoints.size() / THREAD_NUM;
        if(num == 0){
            num = 1;
        }
        ThreadPoolExecutor poolExecutor = new ThreadPoolExecutor(num, num, 0L, TimeUnit.MILLISECONDS, new LinkedBlockingQueue<>());
        CountDownLatch latch = new CountDownLatch(num);
        Map<Integer, List<LonLatDto>> map = new HashMap<>();
        for (int i = 0; i < num; i++) {
            List<LonLatDto> trackPoints = originalPoints.subList(i * THREAD_NUM, i == num - 1 ? originalPoints.size() : (i + 1) * THREAD_NUM);
            int finalI = i;
            poolExecutor.execute(()->{
                List<LonLatDto> new_polyline = new ArrayList<>();
                for (int j = 0; j < trackPoints.size(); j++) {
                    LonLatDto trackPoint = trackPoints.get(j);
                    if(j == trackPoints.size() - 1){
                        new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude()));
                        break;
                    }
                    LonLatDto trackPoint1 = trackPoints.get(j+1);

                    //对两个点位进行坐标补全
                    List<TrackPoint> track = new ArrayList<>();
                    track.add(new TrackPoint(trackPoint.getLatitude(), trackPoint.getLongitude(), null));
                    track.add(new TrackPoint(trackPoint1.getLatitude(), trackPoint1.getLongitude(), null));
                    //10米补全一个点位
                    List<TrackPoint> densified = densifyByDistance(track, distance);
                    for (TrackPoint point : densified) {
                        new_polyline.add(new LonLatDto(point.getLongitude(), point.getLatitude()));
                    }
                }
                new_polyline.remove(new_polyline.size() - 1);
                map.put(finalI, new_polyline);
                latch.countDown();
            });
        }
        try {
            latch.await();
            poolExecutor.shutdown();
        } catch (InterruptedException e) {
            e.printStackTrace();
            throw new RuntimeException(e);
        }
        List<LonLatDto> result = new ArrayList<>();
        for (int i = 0; i < num; i++) {
            result.addAll(map.get(i));
        }
        //添加最后一个点
        result.add(originalPoints.get(originalPoints.size() - 1));
        return result;
    }



    /**
@@ -18,7 +81,7 @@
     * @param distance 补密的间隔距离，例如每 5 米补一个点
     * @return 补密后的轨迹列表
     */
    public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, int distance) {
    public static List<TrackPoint> densifyByDistance(List<TrackPoint> originalPoints, int distance) {
        List<TrackPoint> result = new ArrayList<>();
        if (originalPoints == null || originalPoints.size() < 2) {
            return originalPoints;
@@ -61,7 +124,7 @@
        }

        // 添加最后一个点
        result.add(originalPoints.get(originalPoints.size() - 1));
//        result.add(originalPoints.get(originalPoints.size() - 1));

        return result;
    }
@@ -129,18 +192,29 @@

    // 主函数测试
    public static void main(String[] args) {
        List<TrackPoint> track = new ArrayList<>();
        // 模拟数据：两个点相差 20 秒
        track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
        track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
//        List<TrackPoint> track = new ArrayList<>();
//        // 模拟数据：两个点相差 20 秒
//        track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
//        track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
//
//        // 每 5 秒补一个点
//        List<TrackPoint> densified = densifyByDistance(track, 10);
//
//        System.out.println("原始点数: " + track.size());
//        System.out.println("补密后点数: " + densified.size());
//        System.out.println("轨迹详情:");
//        for (TrackPoint p : densified) {
//            System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
//        }

        // 每 5 秒补一个点
        List<TrackPoint> densified = densifyByTime(track, 10);
        List<LonLatDto> track = new ArrayList<>();
        track.add(new LonLatDto(113.386036, 23.15308));
        track.add(new LonLatDto(113.385671, 23.153252));
        track.add(new LonLatDto(113.385402, 23.153397));
        track.add(new LonLatDto(113.385211, 23.15352));

        System.out.println("原始点数: " + track.size());
        System.out.println("补密后点数: " + densified.size());
        System.out.println("轨迹详情:");
        for (TrackPoint p : densified) {
        List<LonLatDto> densify = densify(track, 10);
        for (LonLatDto p : densify) {
            System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
        }
    }