@aibee/crc-bmap 0.8.39 → 0.8.40

package/lib/src/plugins/car-inertial-position/car-inertial-position.d.ts

@@ -0,0 +1,33 @@
+import { EventDispatcher } from "three";
+import { Compass } from "./compass";
+export interface CarPosition {
+    position: [number, number];
+    time: number;
+    type: "vision" | "beacon";
+}
+export interface CarPosInfo {
+    success: boolean;
+    pos: [number, number];
+    compass: null | number;
+    speed: number;
+}
+interface CarInertialPositionEventMap {
+    'change-compass': {
+        value: number;
+    };
+    'change-pos': {
+        value: CarPosInfo;
+    };
+}
+export declare class CarInertialPosition extends EventDispatcher<CarInertialPositionEventMap> {
+    history: CarPosition[];
+    compass: Compass;
+    compassAngle: number;
+    constructor();
+    startCompass(): void;
+    setPosition(position: CarPosition['position'], time: number): void;
+    setBeaconPosition(position: CarPosition['position'], time: number): void;
+    getPosition(): CarPosInfo;
+    dispose(): void;
+}
+export {};

package/lib/src/plugins/car-inertial-position/car-inertial-position.js

@@ -0,0 +1,90 @@
+// 车辆惯性导航
+import { EventDispatcher } from "three";
+import { Compass } from "./compass";
+import { calculateLineDirection, predictFuturePosition, predictFutureSpeed, transformSpeed } from "./utils";
+export class CarInertialPosition extends EventDispatcher {
+    startCompass() {
+        this.compass.start();
+    }
+    setPosition(position, time) {
+        this.history.push({
+            position,
+            time,
+            type: "vision"
+        });
+        // 保留最近的6条数据
+        while(this.history.length > 6){
+            this.history.shift();
+        }
+        const posInfo = this.getPosition();
+        this.dispatchEvent({
+            type: "change-pos",
+            value: posInfo
+        });
+    }
+    setBeaconPosition(position, time) {
+        this.history.push({
+            position,
+            time,
+            type: "beacon"
+        });
+        const posInfo = this.getPosition();
+        this.dispatchEvent({
+            type: "change-pos",
+            value: posInfo
+        });
+    }
+    getPosition() {
+        if (this.history.length === 0) {
+            return {
+                success: false,
+                pos: [
+                    0,
+                    0
+                ],
+                compass: null,
+                speed: 0
+            };
+        }
+        const lastHistory = this.history.slice(-1)[0];
+        if (this.history.length < 2) {
+            return {
+                success: true,
+                pos: lastHistory.position,
+                compass: null,
+                speed: 0
+            };
+        }
+        const visionHistory = this.history.filter((item)=>item.type === "vision");
+        // 推导到当前时间距离最后一个定位点的时间
+        const angle = calculateLineDirection(visionHistory.map((history)=>history.position));
+        if (angle !== null) {
+            this.compass.setAbsoluteCompass(angle, lastHistory.time);
+        }
+        const lastTime = lastHistory.time;
+        const deltaTime = Date.now() - lastTime;
+        // 预估后的速度
+        const speed = predictFutureSpeed(this.history, deltaTime);
+        // 根据角度和速度时间计算预估后的位置
+        const pos = this.compassAngle ? predictFuturePosition(lastHistory.position, speed, 360 - this.compassAngle, deltaTime) : lastHistory.position;
+        return {
+            success: true,
+            pos,
+            compass: this.compassAngle,
+            speed: transformSpeed(speed)
+        };
+    }
+    dispose() {
+        this.compass.stop();
+    }
+    constructor(){
+        super(), this.history = [], this.compass = new Compass(), this.compassAngle = 0;
+        this.compass.addEventListener("compass", ({ value })=>{
+            this.compassAngle = value;
+            this.dispatchEvent({
+                type: "change-compass",
+                value
+            });
+        });
+    }
+}

package/lib/src/plugins/car-inertial-position/compass.d.ts

@@ -0,0 +1,44 @@
+import { EventDispatcher } from "three";
+interface SensorEventMap {
+    start: {};
+    stop: {};
+    compass: {
+        value: number;
+    };
+}
+interface CompassData {
+    timestamp: number;
+    res: number;
+}
+/**
+ * 监听compass 在获取到视觉定位的角度之后，计算视觉和compass的误差，保存一下这个误差，在后续定位中不断的调整这个误差
+ */
+export declare class Compass extends EventDispatcher<SensorEventMap> {
+    compassData: CompassData[];
+    absoluteCompass: null | {
+        time: number;
+        compass: number;
+    };
+    delta: number;
+    deltas: number[];
+    constructor();
+    start(): void;
+    deviceOrientationAbsHandler: (e: DeviceOrientationEvent) => void;
+    /**
+     * 设置一个根据视觉定位返回的绝对角度 校准delta
+     * @param compass
+     * @param time
+     */
+    setAbsoluteCompass(compass: number, time: number): void;
+    emitCompass(compass: number): void;
+    /**
+     * 获取 compass 和 deviceMotion
+     */
+    listenDeviceOrientation(): void;
+    checkSensor(): Promise<{
+        deviceOrientation: boolean;
+    }>;
+    checkDeviceOrientation(): Promise<boolean>;
+    stop(): void;
+}
+export {};

package/lib/src/plugins/car-inertial-position/compass.js

@@ -0,0 +1,119 @@
+import { _ as _async_to_generator } from "@swc/helpers/_/_async_to_generator";
+import { isNil } from "lodash";
+import { isIphone } from "../../utils";
+import { EventDispatcher } from "three";
+import { removeOutliers } from "./utils";
+/**
+ * 监听compass 在获取到视觉定位的角度之后，计算视觉和compass的误差，保存一下这个误差，在后续定位中不断的调整这个误差
+ */ export class Compass extends EventDispatcher {
+    start() {
+        this.listenDeviceOrientation();
+        this.dispatchEvent({
+            type: "start"
+        });
+    }
+    /**
+   * 设置一个根据视觉定位返回的绝对角度 校准delta
+   * @param compass
+   * @param time
+   */ setAbsoluteCompass(compass, time) {
+        if (!this.compassData.length) {
+            return;
+        }
+        this.absoluteCompass = {
+            compass: compass,
+            time
+        };
+        let cutTimeCompassIndex = this.compassData.findIndex((item)=>item.timestamp >= time);
+        if (cutTimeCompassIndex === -1) {
+            cutTimeCompassIndex = this.compassData.length - 1;
+        }
+        const prevCompass = this.compassData[cutTimeCompassIndex - 1];
+        const nextCompass = this.compassData[cutTimeCompassIndex];
+        let curCompass;
+        if (!prevCompass) {
+            curCompass = nextCompass;
+        } else if (!nextCompass) {
+            return;
+        } else {
+            curCompass = nextCompass.timestamp - time > prevCompass.timestamp - time ? prevCompass : nextCompass;
+        }
+        const delta = compass - curCompass.res;
+        this.deltas.push(delta);
+        const deltas = removeOutliers(this.deltas);
+        this.delta = deltas.reduce((sum, cur)=>sum + cur, 0) / deltas.length;
+    }
+    emitCompass(compass) {
+        // 把 compass 限制在 0 ~ 360 
+        this.dispatchEvent({
+            type: "compass",
+            value: (compass + this.delta + 360) % 360
+        });
+    }
+    /**
+   * 获取 compass 和 deviceMotion
+   */ listenDeviceOrientation() {
+        if (isIphone) {
+            window.addEventListener("deviceorientation", this.deviceOrientationAbsHandler, false);
+        } else {
+            window.addEventListener("deviceorientationabsolute", this.deviceOrientationAbsHandler, {
+                absolute: true
+            });
+        }
+    }
+    checkSensor() {
+        var _this = this;
+        return _async_to_generator(function*() {
+            const deviceOrientation = yield _this.checkDeviceOrientation();
+            return {
+                deviceOrientation
+            };
+        })();
+    }
+    checkDeviceOrientation() {
+        return _async_to_generator(function*() {
+            var _window_DeviceOrientationEvent;
+            if (!isIphone) {
+                return true;
+            }
+            if (typeof window.DeviceOrientationEvent !== "undefined" && typeof ((_window_DeviceOrientationEvent = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent.requestPermission) === "function") {
+                try {
+                    var _window_DeviceOrientationEvent1;
+                    const permission = yield (_window_DeviceOrientationEvent1 = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent1.requestPermission();
+                    return permission === "granted";
+                } catch (e) {
+                    return false;
+                }
+            }
+            return false;
+        })();
+    }
+    stop() {
+        if (isIphone) {
+            window.removeEventListener("deviceorientation", this.deviceOrientationAbsHandler, false);
+        } else {
+            window.removeEventListener("deviceorientationabsolute", this.deviceOrientationAbsHandler, {
+                absolute: true
+            });
+        }
+        this.compassData = [];
+        this.dispatchEvent({
+            type: "stop"
+        });
+    }
+    constructor(){
+        super(), this.compassData = [], this.absoluteCompass = null, this.delta = 0, this.deltas = [], this.deviceOrientationAbsHandler = (e)=>{
+            const currTime = Date.now();
+            const { alpha, beta, gamma } = e;
+            if (isNil(alpha) || isNil(beta) || isNil(gamma)) {
+                return;
+            }
+            const compass = isIphone ? e.webkitCompassHeading : 360 - alpha;
+            this.compassData.push({
+                timestamp: currTime,
+                res: compass
+            });
+            this.emitCompass(compass);
+        };
+    }
+}

package/lib/src/plugins/car-inertial-position/index.d.ts

@@ -0,0 +1,2 @@
+export * from './car-inertial-position';
+export * from './utils';

package/lib/src/plugins/car-inertial-position/index.js

@@ -0,0 +1,3 @@
+// 车辆惯性导航
+export * from './car-inertial-position';
+export * from './utils';

package/lib/src/plugins/car-inertial-position/utils.d.ts

@@ -0,0 +1,41 @@
+import { CarPosition } from "./car-inertial-position";
+/**
+ * 计算一组点位的斜率和方向
+ * @param points
+ * @returns
+ */
+export declare function calculateLineDirection(points: [number, number][]): number | null;
+/**
+ * 计算速度，单位是m/ms km/s
+ * @param positions
+ * @returns
+ */
+export declare function calculateInstantaneousSpeed(positions: CarPosition[]): number[];
+/**
+ * 预测一段时间后的速度
+ * @param speeds
+ * @param timeIntervalMs
+ * @returns
+ */
+export declare function predictFutureSpeed(positions: CarPosition[], timeIntervalMs: number): number;
+/**
+ * 转换速度从 m/ms 转成  km/h
+ * @param speed
+ * @returns
+ */
+export declare function transformSpeed(speed: number): number;
+/**
+ * 预估未来的位置
+ * @param lastPosition
+ * @param speed
+ * @param angle
+ * @param time
+ * @returns
+ */
+export declare function predictFuturePosition(lastPosition: [number, number], speed: number, angle: number, time: number): [number, number];
+/**
+ * 过滤偏差大的值
+ * @param data
+ * @returns
+ */
+export declare function removeOutliers(data: number[]): number[];

package/lib/src/plugins/car-inertial-position/utils.js

@@ -0,0 +1,150 @@
+import { Vector2 } from "three";
+/**
+ * 计算一组点位的斜率和方向
+ * @param points
+ * @returns
+ */ export function calculateLineDirection(points) {
+    const n = points.length;
+    if (n < 2) {
+        return null;
+    }
+    const angles = [];
+    for(let i = 1; i < points.length; i++){
+        const point1 = new Vector2(points[i][0], points[i][1]);
+        const point0 = new Vector2(points[i - 1][0], points[i - 1][1]);
+        if (point1.distanceTo(point0) === 0) {
+            continue;
+        }
+        const radians = new Vector2().subVectors(point1, point0).angle() // 于X轴的正方向的夹角 这个是顺时针的角度
+        ;
+        const angle = 360 - (radians / Math.PI * 180 - 90 + 360) % 360;
+        angles.push(angle);
+    }
+    const filteredAngles = removeOutliers(angles);
+    if (!filteredAngles.length) {
+        return null;
+    }
+    let weight = 0;
+    // 最近的方向权重更大
+    const averageAngle = filteredAngles.reduce((sum, angle, i)=>{
+        weight += i + 1;
+        return sum + angle * (i + 1);
+    }, 0) / weight;
+    return averageAngle;
+}
+/**
+ * 计算速度，单位是m/ms km/s
+ * @param positions
+ * @returns
+ */ export function calculateInstantaneousSpeed(positions) {
+    const speeds = [];
+    for(let i = 1; i < positions.length; i++){
+        const prev = positions[i - 1];
+        const current = positions[i];
+        // 计算位置变化
+        const deltaX = current.position[0] - prev.position[0];
+        const deltaY = current.position[1] - prev.position[1];
+        const distance = Math.sqrt(deltaX ** 2 + deltaY ** 2);
+        // 计算时间变化
+        const deltaTime = current.time - prev.time; // 时间单位为毫秒
+        // 计算瞬时速度
+        if (deltaTime > 0) {
+            const speed = distance / deltaTime;
+            speeds.push(speed);
+        } else {
+            speeds.push(0); // 如果时间变化为0，速度为0
+        }
+    }
+    return speeds;
+}
+/**
+ * 预测一段时间后的速度
+ * @param speeds
+ * @param timeIntervalMs
+ * @returns
+ */ export function predictFutureSpeed(positions, timeIntervalMs) {
+    const speeds = calculateInstantaneousSpeed(positions);
+    const n = speeds.length;
+    if (n === 0) {
+        return 0; // 如果没有速度数据，返回0
+    }
+    // 获取当前速度
+    const currentSpeed = speeds[n - 1];
+    // 计算加速度（简单的平均加速度）
+    let acceleration = 0;
+    if (n > 1) {
+        const speedDifferences = speeds.slice(1).map((speed, index)=>speed - speeds[index]);
+        const time = positions.slice(1).reduce((sum, cur, index)=>sum + (cur.time - positions[index].time), 0);
+        acceleration = speedDifferences.reduce((sum, value)=>sum + value, 0) / time;
+    }
+    // 预测未来速度 m/ms
+    const futureSpeed = Math.max(currentSpeed + acceleration * timeIntervalMs, 0);
+    return futureSpeed;
+}
+/**
+ * 转换速度从 m/ms 转成  km/h
+ * @param speed
+ * @returns
+ */ export function transformSpeed(speed) {
+    // speed m/ms  km/s
+    return speed * 60 * 60; // km/h
+}
+/**
+ * 预估未来的位置
+ * @param lastPosition
+ * @param speed
+ * @param angle
+ * @param time
+ * @returns
+ */ export function predictFuturePosition(lastPosition, speed, angle, time) {
+    // 将角度转换为弧度
+    const angleInRadians = angle * (Math.PI / 180);
+    const distance = speed * time;
+    console.log("angle speed", speed, distance);
+    const vec = new Vector2(lastPosition[0], lastPosition[1]);
+    // 计算角度在x和y方向的分量
+    const vX = Math.cos(angleInRadians);
+    const vY = Math.sin(angleInRadians);
+    const dir = new Vector2(vX, vY);
+    vec.add(dir.normalize().multiplyScalar(distance));
+    // // 计算未来位置
+    // const futureX = lastPosition[0] + vX * time;
+    // const futureY = lastPosition[1] + vY * time;
+    return [
+        vec.x,
+        vec.y
+    ];
+}
+/**
+ * 过滤偏差大的值
+ * @param data 
+ * @returns 
+ */ export function removeOutliers(data) {
+    // 排序数据
+    const d = [
+        ...data
+    ].sort((a, b)=>a - b);
+    // 计算四分位数 Q1 和 Q3
+    const q1 = percentile(d, 25);
+    const q3 = percentile(d, 75);
+    // 计算四分位距 IQR
+    const iqr = q3 - q1;
+    // 计算上下界限
+    const lowerBound = q1 - 1.5 * iqr;
+    const upperBound = q3 + 1.5 * iqr;
+    // 筛选出正常范围内的数据
+    const filteredData = d.filter((value)=>value >= lowerBound && value <= upperBound);
+    const originSortData = data.filter((item)=>filteredData.includes(item));
+    return originSortData;
+}
+// 计算百分位数（分位数）
+function percentile(arr, p) {
+    const index = p / 100 * (arr.length - 1);
+    const lower = Math.floor(index);
+    const upper = Math.ceil(index);
+    if (lower === upper) {
+        return arr[lower];
+    }
+    const weight = index - lower;
+    return arr[lower] * (1 - weight) + arr[upper] * weight;
+}

package/lib/src/plugins/index.d.ts

@@ -7,3 +7,4 @@ export * from './select';
 export * from './pdr-position';
 export * from './mul-floor-navigation';
 export * from './mul-floor-select';
+export * from './car-inertial-position';

package/lib/src/plugins/index.js

@@ -7,3 +7,4 @@ export * from './select';
 export * from './pdr-position';
 export * from './mul-floor-navigation';
 export * from './mul-floor-select';
+export * from './car-inertial-position';

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@aibee/crc-bmap",
-  "version": "0.8.39",
+  "version": "0.8.40",
   "description": "",
   "main": "lib/bmap.min.js",
   "module": "lib/bmap.esm.js",