VitePress

Appearance

叠帧优化 ​

为了提高对静态物的标注效率，对叠帧算法进行更新。经过叠帧算法计算的静态物位置，标准员无需做任何修改。

前端交互优化 ​

  • Before: 叠帧1.0, 先对点云进行叠帧，再将在叠帧点云上绘制的框体映射到每一帧。
  • After: 叠帧2.0, 无需对点云进行叠帧，直接根据当前帧点云绘制的框体位置计算其他帧框体位置。

关于矩阵 ​

矩阵转换之左乘 ​

C * B * A * POINT => 对 POINT 依次进行 A B C 矩阵左乘变换

js
POINT.clone().multiply(A).multiply(B).multiply(C)

矩阵的逆矩阵 ​

matrix.clone().invert()， 逆向变换：局部坐标转全局坐标 与 全局坐标转局部坐标

矩阵的转置 ​

matrix.clone().invert()， 使用一维数组和二维数组表示矩阵的时候，得到矩阵可能存在一个转置的关系

叠帧算法 ​

叠帧算法最核心的地方就是对参数的理解

  • 点云所在的坐标系是自车的坐标系

  • 位姿信息的坐标系是 gnss 的坐标系

  • 旋转的顺序：yaw -> pitch -> roll

  • 局部坐标转全局坐标是：先旋转后平移

  • 第一帧局部坐标转全局坐标计算顺序：global = T * R * gnss2lidar_i * lidar2car_i * point1

  • 第二帧全局坐标转局部坐标计算顺序：point2 = lidar2car * gnss2lidar * R_i * T_i * global

alt text

算法一：合并转换矩阵 ​

js
/**
 *
 * @param pos 当前帧位置信息 vector3
 * @param currentFrameConfig 当前帧自车位置信息
 * @param targetFrameConfig  目标帧自车位置信息
 */
export function positionMapping(
    pos: THREE.Vector3 | THREE.Vector3[],
    currentFrameConfig: PositionInfoType,
    targetFrameConfig: PositionInfoType,
) {
    if (!pos || !currentFrameConfig || !targetFrameConfig) return false;
    pos = Array.isArray(pos) ? pos : [pos];

    function overlapP2P(point: THREE.Vector3, currentFrameConfig: PositionInfoType, targetFrameConfig: PositionInfoType) {
        const gnss2lidarArr = [
            [
                -0.026109514521249981,
                0.99965286595866976,
                -0.0036201203852738446,
                -0.047121363808201665
            ],
            [
                -0.99957277620278817,
                -0.026059425187690454,
                0.013241260141465558,
                -0.037917682102624306
            ],
            [
                0.013142390807446535,
                0.0039642986731841176,
                0.99990538314506994,
                -0.16919957365882846
            ],
            [
                0,
                0,
                0,
                1
            ]
        ];

        const gnss2lidar = currentFrameConfig.gnss2lidar ? currentFrameConfig.gnss2lidar : gnss2lidarArr;
        const lidar2car = currentFrameConfig.lidar2car;

        const gnss2lidarMat = new THREE.Matrix4().fromArray(gnss2lidar.flat()).transpose();

        const lidar2carMat = new THREE.Matrix4().fromArray(lidar2car.flat()).transpose();

        const gnss2car = new THREE.Matrix4().multiplyMatrices(lidar2carMat, gnss2lidarMat);
    
        // Helper: Create a 4x4 transformation matrix from position (XYZRPY)
        function createTransformMatrix(position) {
            const degToRad = Math.PI / 180;
            const roll = position.roll * degToRad;
            const pitch = -position.pitch * degToRad;
            const yaw = (90 - position.yaw) * degToRad;
            console.log({ roll, pitch, yaw });
    
            // Create transformation matrix
            const matrix = new THREE.Matrix4();
            matrix.makeRotationFromEuler(new THREE.Euler(roll, pitch, yaw, 'ZYX'));
            // matrix.makeRotationFromQuaternion(rotationQuaternion);
            matrix.setPosition(new THREE.Vector3(position.x, position.y, position.z));
    
            return matrix;
        }
    
        // Generate transformation matrices for position1 and position2
        const T1 = createTransformMatrix(currentFrameConfig.position);
        const T2 = createTransformMatrix(targetFrameConfig.position);

        // Invert GNSS to Car matrix
        const gnss2carInv = new THREE.Matrix4().copy(gnss2car).invert();

        // Compute T1 in world coordinates
        const transformT1 = new THREE.Matrix4().multiplyMatrices(T1, gnss2carInv);

        // Point in T1 frame
        const pose1 = new THREE.Vector4(point.x, point.y, point.z, 1);
        const pointWorld = pose1.clone().applyMatrix4(transformT1);

        // Compute T2 in world coordinates
        const transformT2 = new THREE.Matrix4().multiplyMatrices(T2, gnss2carInv);
        const transformT2Inv = new THREE.Matrix4().copy(transformT2).invert();

        // Transform point from world to T2 frame
        const pose2 = pointWorld.clone().applyMatrix4(transformT2Inv);

        // return [pointT2.x, pointT2.y, pointT2.z];
        return new THREE.Vector3(pose2.x, pose2.y, pose2.z);
    }
    
    return pos.map((p) => overlapP2P(p, currentFrameConfig, targetFrameConfig));
}

/**
 *
 * @param rotateion euler
 * @param currentFrameConfig 当前帧自车位置信息
 * @param targetFrameConfig  目标帧自车位置信息
 */
export function rotationMapping(
    rotateion: THREE.Euler,
    currentFrameConfig: PositionInfoType,
    targetFrameConfig: PositionInfoType,
) {
    if (!rotateion || !currentFrameConfig || !targetFrameConfig) return false;
    let { yaw: c_yaw, pitch: c_pitch, roll: c_roll } = currentFrameConfig.position;
    let { yaw: t_yaw, pitch: t_pitch, roll: t_roll } = targetFrameConfig.position;
    let newRotation = rotateion.clone();
    c_yaw = (c_yaw * Math.PI) / 180;
    c_pitch = (c_pitch * Math.PI) / 180;
    c_roll = (c_roll * Math.PI) / 180;

    t_yaw = (t_yaw * Math.PI) / 180;
    t_pitch = (t_pitch * Math.PI) / 180;
    t_roll = (t_roll * Math.PI) / 180;
    newRotation.set(rotateion.x + t_roll - c_roll, rotateion.y + t_pitch - c_pitch, rotateion.z + t_yaw - c_yaw);
    return newRotation;
}

算法二: 逐步拆解 ​

js
/**
 *
 * @param pos 当前帧位置信息 vector3
 * @param currentFrameConfig 当前帧自车位置信息
 * @param targetFrameConfig  目标帧自车位置信息
 */
export function positionMappingV2(
    pos: THREE.Vector3 | THREE.Vector3[],
    currentFrameConfig: PositionInfoType,
    targetFrameConfig: PositionInfoType,
) {
    if (!pos || !currentFrameConfig || !targetFrameConfig) return false;
    pos = Array.isArray(pos) ? pos : [pos];

    let {
        yaw: c_yaw, 
        pitch: c_pitch, 
        roll: c_roll, 
        x: c_x, 
        y: c_y, 
        z: c_z 
    } = currentFrameConfig.position;

    //角度转弧度
    // c_yaw = c_yaw > 270 ? 450 - c_yaw : 90 - c_yaw;
    c_yaw = (c_yaw * Math.PI) / 180;
    c_yaw = Math.PI / 2 - c_yaw;
    c_pitch = -(c_pitch * Math.PI) / 180;
    c_roll = (c_roll * Math.PI) / 180;
    let c_pos = new THREE.Vector3(c_x, c_y, c_z);
    //当前帧到指北坐标系的矩阵
    
    const dir = 'ZYX';
    let c_quaternion = new THREE.Quaternion();
    c_quaternion.setFromEuler(new THREE.Euler(c_roll, c_pitch, c_yaw, dir));
    // c_quaternion.setFromEuler(new THREE.Euler(0, 0, c_yaw, 'ZYX'));
    let {
        yaw: t_yaw, 
        pitch: t_pitch, 
        roll: t_roll, 
        x: t_x, 
        y: t_y, 
        z: t_z 
    } = targetFrameConfig.position;
    //目标帧的角度转弧度
    // t_yaw = t_yaw > 270 ? 450 - t_yaw : 90 - t_yaw;
    t_yaw = (t_yaw * Math.PI) / 180;
    t_yaw = Math.PI / 2 - t_yaw;
    t_pitch = -(t_pitch * Math.PI) / 180;
    t_roll = (t_roll * Math.PI) / 180;
    let t_pos = new THREE.Vector3(t_x, t_y, t_z);
    //目标帧到指北角的矩阵
    let t_quaternion = new THREE.Quaternion();
    t_quaternion.setFromEuler(new THREE.Euler(t_roll, t_pitch, t_yaw, dir));
    let t_quaternion_inverted = t_quaternion.invert();

    let c_matrix4 = new THREE.Matrix4();
    c_matrix4.setPosition(c_pos);
    let t_matrix4 = new THREE.Matrix4();
    t_matrix4.setPosition(t_pos).invert();
    let c_lidar2car = new THREE.Matrix4().fromArray(currentFrameConfig.lidar2car.flat()).transpose();
    let t_lidar2car_i = c_lidar2car.clone().invert();
    const gnss2lidarArr = [
        [
            -0.026109514521249981,
            0.99965286595866976,
            -0.0036201203852738446,
            -0.047121363808201665
        ],
        [
            -0.99957277620278817,
            -0.026059425187690454,
            0.013241260141465558,
            -0.037917682102624306
        ],
        [
            0.013142390807446535,
            0.0039642986731841176,
            0.99990538314506994,
            -0.16919957365882846
        ],
        [
            0,
            0,
            0,
            1
        ]
    ];

    let gnss2lidar = new THREE.Matrix4().fromArray(gnss2lidarArr.flat()).transpose();
    let gnss2lidar_i = gnss2lidar.clone().invert();
    const gnss2car = new THREE.Matrix4().multiplyMatrices(c_lidar2car, gnss2lidar);
    const gnss2car_i = gnss2car.clone().invert();
    console.log({ gnss2car})
   
    return pos.map((p) => {
        const newPos = p.clone()
            // 局部坐标转全局坐标
            .applyMatrix4(t_lidar2car_i.clone())
            .applyMatrix4(gnss2lidar_i.clone())
            .applyQuaternion(c_quaternion.clone())
            .applyMatrix4(c_matrix4.clone())
            // 全局坐标转局部坐标
            .applyMatrix4(t_matrix4.clone())
            .applyQuaternion(t_quaternion_inverted.clone())
            .applyMatrix4(gnss2lidar.clone())
            .applyMatrix4(c_lidar2car.clone());
        return newPos;
    });
}

算法三：LLA -> ECEF -> ENU（该方法暂时没跑通） ​

js
import * as THREE from 'three';
import proj4  from 'proj4 ';
import { PositionInfoType } from '../type';

/**
 *
 * @param pos 当前帧位置信息 vector3
 * @param currentFrameConfig 当前帧自车位置信息
 * @param targetFrameConfig  目标帧自车位置信息
 */
export function positionMapping(
    pos: THREE.Vector3 | THREE.Vector3[],
    currentFrameConfig: PositionInfoType,
    targetFrameConfig: PositionInfoType,
) {
    if (!pos || !currentFrameConfig || !targetFrameConfig) return false;
    pos = Array.isArray(pos) ? pos : [pos];
    const utmProj = "+proj=utm +zone=49 +datum=WGS84 +units=m +no_defs"; 
    const wgs84 = "+proj=longlat +datum=WGS84 +no_defs";

    function llaToECEF(lat: number, lon: number, h: number) {
        const a = 6378137.0; // WGS84椭球的长半轴 (米)
        const e2 = 0.00669437999014; // WGS84椭球的偏心率的平方
    
        const N = a / Math.sqrt(1 - e2 * Math.sin(lat) * Math.sin(lat));
    
        const x = (N + h) * Math.cos(lat) * Math.cos(lon);
        const y = (N + h) * Math.cos(lat) * Math.sin(lon);
        const z = ((1 - e2) * N + h) * Math.sin(lat);
    
        return { x, y, z };
    }
    
    function ecefToENU(lat0: number, lon0: number, h0: number, lat: number, lon: number, h: number) {
        // 参考点的ECEF坐标
        const ecef0 = llaToECEF(lat0, lon0, h0);
        const ecef = llaToECEF(lat, lon, h);
    
        const dx = ecef.x - ecef0.x;
        const dy = ecef.y - ecef0.y;
        const dz = ecef.z - ecef0.z;
    
        // 计算参考点的ECEF坐标系的旋转矩阵
        const sinLat0 = Math.sin(lat0);
        const cosLat0 = Math.cos(lat0);
        const sinLon0 = Math.sin(lon0);
        const cosLon0 = Math.cos(lon0);
    
        // ENU坐标
        const e = -sinLon0 * dx + cosLon0 * dy;
        const n = -sinLat0 * cosLon0 * dx - sinLat0 * sinLon0 * dy + cosLat0 * dz;
        const u = cosLat0 * cosLon0 * dx + cosLat0 * sinLon0 * dy + sinLat0 * dz;
    
        return new THREE.Vector4( e, n, u, 1 );
    }

    function calTrans(c_x: number, c_y: number, c_z: number, t_x: number, t_y: number, t_z: number) {
        // 转换为经纬度
        const [c_lon, c_lat] = proj4(utmProj, wgs84, [c_x, c_y]);
        const [t_lon, t_lat] = proj4(utmProj, wgs84, [t_x, t_y]);
        console.log({c_lon, c_lat, t_lon, t_lat});
        const c_lat_r = THREE.MathUtils.degToRad(c_lat);
        const c_lon_r = THREE.MathUtils.degToRad(c_lon);
        const t_lat_r = THREE.MathUtils.degToRad(t_lat);
        const t_lon_r = THREE.MathUtils.degToRad(t_lon);
        return ecefToENU(c_lat_r, c_lon_r, c_z, t_lat_r, t_lon_r, t_z);
    }

    // Create transformation matrices for A and B
    let {
        yaw: c_yaw,
        pitch: c_pitch,
        roll: c_roll,
        x: c_x,
        y: c_y,
        z: c_z
    } = currentFrameConfig.position;
    let {
        yaw: t_yaw,
        pitch: t_pitch,
        roll: t_roll, 
        x: t_x,
        y: t_y,
        z: t_z
    } = targetFrameConfig.position;

    const gnss2lidarArr = [
        [
            -0.026109514521249981,
            0.99965286595866976,
            -0.0036201203852738446,
            -0.047121363808201665
        ],
        [
            -0.99957277620278817,
            -0.026059425187690454,
            0.013241260141465558,
            -0.037917682102624306
        ],
        [
            0.013142390807446535,
            0.0039642986731841176,
            0.99990538314506994,
            -0.16919957365882846
        ],
        [
            0,
            0,
            0,
            1
        ]
    ];

    let c_lidar2car = new THREE.Matrix4().fromArray(currentFrameConfig.lidar2car.flat()).transpose();
    let t_lidar2car_i = c_lidar2car.clone().invert();
    let gnss2lidar = new THREE.Matrix4().fromArray(gnss2lidarArr.flat()).transpose();
    let gnss2lidar_i = gnss2lidar.clone().invert();
    const gnss2car = new THREE.Matrix4().multiplyMatrices(c_lidar2car, gnss2lidar);
    const gnss2car_i = gnss2car.clone().invert();
    console.log({ gnss2car})
    const pose1 = new THREE.Vector3(c_x, c_y, c_z).clone().applyMatrix4(gnss2car_i);
    const pose2 = new THREE.Vector3(t_x, t_y, t_z).clone().applyMatrix4(gnss2car_i);
    const trans = calTrans(pose1.x, pose1.y, pose1.z, pose2.x, pose2.y, pose2.z)
    return pos.map((p) => {
        const newPos = p.clone().sub(trans);
        return newPos;
    });
}