Skip to content

Instantly share code, notes, and snippets.

@cipharius

cipharius/plane-fit.nim

Created August 19, 2018 12:01
Show Gist options
  • Save cipharius/bdad8144ed573dec51146fe1c83444e0 to your computer and use it in GitHub Desktop.
Save cipharius/bdad8144ed573dec51146fe1c83444e0 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. cipharius created this gist Aug 19, 2018.
    37 changes: 37 additions & 0 deletions plane-fit.nim
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,37 @@
    proc planeFit(cluster: Tensor[float], threshold = 10e-2): Option[Plane] =
    ## Fit points to a 3D plane
    # At least 3 points are required to fit a plane
    if cluster.shape[0] < 3: return

    let points = cluster[_, 0..2]
    let mean = points.mean(axis=0)
    let delta = points .- mean

    let covarMatrix = delta.covariance
    let (eigenVal, eigenVec) = covarMatrix.symeig(true)
    discard eigenVal

    var planeNormal = eigenVec[_, 0].squeeze

    # Filter out plane points
    var i = 0
    var planeDelta = zeros_like delta
    for slice in delta.axis(0):
    let norm = slice.squeeze / slice.dist
    if norm.dot(planeNormal).abs < threshold:
    planeDelta[i, _] = slice
    inc i
    planeDelta = planeDelta[0..<i, _]

    let origin = mean + (planeDelta.min(axis=0) + planeDelta.max(axis=0)) / 2.0

    # Plane normal orientation correction
    let originOffset = (origin - mean).squeeze
    if originOffset.dist > 0:
    let norm = originOffset / originOffset.dist
    planeNormal *= norm.dot(planeNormal).sgn.float

    return some Plane(
    origin: origin.squeeze,
    normal: planeNormal
    )