plane-fit.nim

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
  )