instructions

In practical projects, we sometimes measure the attitude matrix of an object multiple times, or measure it simultaneously from multiple different reference points. These measurements may end up being close, but not identical, and we hope to combine these results to produce a theoretically more accurate “mean matrix.”

From a mathematical point of view, any averaging method is acceptable, but we are simply calculating a more intuitive “averaging matrix” for practical engineering purposes.

The geometric

If we’re measuring the position of an object, and we get a lot of results, we can just take the geometric average of those points, or the least square points.

So if you end up with a bunch of matrices, you’re going to wonder: Just add the matrices and take the average? The answer to this question is available on the web, and it’s generally good to average quaternions for rotation matrices. How to obtain average rotation with multiple rotation matrices? – Fly QQ answer – Zhihu

(a) Euler Angle averaging; (b) Quaternion averaging; (c) Mean in tangent space; (d) Numerical solution results

Then we also need to solve the scaling problem. Since the scaling multiple of each axis may change after rotation, we assume that the scaling is uniform, that is, scaleX = scaleY = scaleZ. So you need to figure out a uniform scale.

So how do you measure the scale of a matrix? In geometry, it can be understood as the volume of a cube (or parallelotetrahedron) composed of x, y and Z axes, while in algebra, it can be expressed by the value of the determinant of a matrix.

For a cube, you have to take the third root of the volume to get the side length. The square root operation should be avoided whenever possible. So you can provide a function method without scaling.

For a 3 by 3 matrix, you can just take the determinant, and for a 4 by 4 matrix you have to take the determinant of the 3 by 3 matrix in the upper left corner. But!!!!! The 4×4 matrix we’re using is very special, the last vector in each column is 0, and those 0’s are multiplied by the first three terms in the last column, cancelling out these shifts. From a geometric point of view: translation does not change the volume of the cube (or parallelotetrahedron) composed of x, Y and Z axes.

code

static func averageMatrix(_ matrices:[simd_float4x4], needScale:Bool = false) -> simd_float4x4 {
    var averagePosition = simd_float4.zero
    var averageScale:Float = 0
    var averageRotation = simd_quatf()
    
    for matrix in matrices {
        averagePosition + = matrix.columns.3
        if needScale {
            let d = matrix.determinant
            averageScale + = cbrtf(d)
        }
        averageRotation + = simd_quatf(matrix)
    }
    
    let count = Float(matrices.count)
    averagePosition / = count
    averageRotation = averageRotation.normalized
    if needScale {
        averageScale / = count
        let s = simd_float4x4(diagonal: simd_float4(averageScale, averageScale, averageScale, 1))
        let r = simd_float4x4(averageRotation)
        let t = simd_float4x4(translation: averagePosition)
        // Composite transformation convention order: scale - > rotate - > pan
        return t * r * s
    } else {
        let r = simd_float4x4(averageRotation)
        let t = simd_float4x4(translation: averagePosition)
        // Composite transformation convention order: scale - > rotate - > pan
        return t * r
    }
}
Copy the code