图形拟合（三）：椭圆拟合

算法背景

参考论文实现《Direct least square fitting of ellipses》

OpenCV中也有对应实现，可参考文档：

https://docs.opencv.org/3.4/d3/dc0/group__imgproc__shape.html#gaf259efaad93098103d6c27b9e4900ffa

JavaScript版本实现

JavaScript版本仅实现了 fitEllipse中 的 fitEllipseNoDirect 部分。

class Point {
  x : number
  y : number
  constructor(_x : number, _y : number) {
    this.x = _x
    this.y = _y
  }
}

class MiniOpenCV {

  static getOfs(i: number, eps: number) {
    return new Point(((i & 1)*2 - 1)*eps, ((i & 2) - 1)*eps);
  }

  static fitEllipseNoDirect(_points: Point[]) {
    let points = _points;
    let i, n = points.length;
    if (n < 5) {
      console.error("There should be at least 5 points to fit the ellipse");
      return
    }
    const FLT_EPSILON = 1.19209e-07
    let box = {
      center: {
        x: 0,
        y: 0,
      },
      size: {
        width: 0,
        height: 0
      },
      angle: 0
    }

    let c = new Point(0,0);
    let t;
    let gfp = new Array(5).fill(0)
    let rp = new Array(5).fill(0)
    let vd = new Array(25).fill(0)
    let wd = new Array(5).fill(0)
    const min_eps = 1e-8

    let _AD = new Array(n * 12 + n).fill(0);
    const Ad = 0
    const ud = Ad + 5 * n
    const bd = ud + 5 * n
    const ptsf_copy = bd + n;

    let A = null
    let b = null
    let x = null
    let u = null
    let vt = null
    let w = null

    {
      const ptsf = points;
      for (i = 0; i < n; i++) {
        let p = ptsf[i]
        _AD[ptsf_copy + i] = p;
        c.x += p.x;
        c.y += p.y;
      }
    }
    c.x /= n;
    c.y /= n;
    
    let s = 0;
    for (i = 0; i < n; i++) {
      let p = JSON.parse(JSON.stringify(_AD[ptsf_copy + i]));
      p.x -= c.x;
      p.y -= c.y;
      s += Math.abs(p.x) + Math.abs(p.y);
    }
    let scale = 100 / (s > FLT_EPSILON ? s : FLT_EPSILON);

    for (i = 0; i < n; i++) {
      let p = JSON.parse(JSON.stringify(_AD[ptsf_copy + i]));
      p.x -= c.x;
      p.y -= c.y;
      let px = p.x * scale;
      let py = p.y * scale;

      _AD[bd + i] = 10000; // 1.0?
      _AD[Ad + i*5] = -px * px; // A - C signs inverted as proposed by APP
      _AD[Ad + i*5 + 1] = -py * py;
      _AD[Ad + i*5 + 2] = -px * py;
      _AD[Ad + i*5 + 3] = px;
      _AD[Ad + i*5 + 4] = py;
    }

    A = Mat.matrix(n, 5, _AD)
    
    let { E, U, V } = Mat.svd(A)
    w = E
    u = U
    vt = Mat.transpose(V)

    let count = 0;
    Mat.forEach(Mat.diag(w), (item: any) => {
      wd[count++] = item;
    })

    if (wd[0] * FLT_EPSILON > wd[4]) {
      let eps = (s / (n * 2) * 1e-3);
      for (i = 0; i < n; i++) {
        let err = MiniOpenCV.getOfs(i, eps);
        let p = _AD[ptsf_copy + i];
        p.x += err.x;
        p.y += err.y;
      }

      for (i = 0; i < n; i++) {
        let p = JSON.parse(JSON.stringify(_AD[ptsf_copy + i]));
        p.x -= c.x;
        p.y -= c.y;
        let px = p.x * scale;
        let py = p.y * scale;
        _AD[bd + i] = 10000.0; // 1.0?
        _AD[Ad + i*5] = -px * px; // A - C signs inverted as proposed by APP
        _AD[Ad + i*5 + 1] = -py * py;
        _AD[Ad + i*5 + 2] = -px * py;
        _AD[Ad + i*5 + 3] = px;
        _AD[Ad + i*5 + 4] = py;
      }

      A = Mat.matrix(n, 5, _AD)
    
      let { E, U, V } = Mat.svd(A)
      w = E
      u = U
      vt = Mat.transpose(V)
    }

    b = Mat.matrix(n, 1, _AD.slice(bd))
    x = Mat.backSubst(w, u, vt, b);

    count = 0;
    Mat.forEach(x, (item: any) => {
      gfp[count++] = item;
    })

    // now use general-form parameters A - E to find the ellipse center:
    // differentiate general form wrt x/y to get two equations for cx and cy
    _AD[Ad + 0] = 2 * gfp[0];
    _AD[Ad + 1] = _AD[Ad + 2] = gfp[2];
    _AD[Ad + 3] = 2 * gfp[1];
    _AD[bd + 0] = gfp[3];
    _AD[bd + 1] = gfp[4];
    A = Mat.matrix(2, 2, _AD);
    b = Mat.matrix(2, 1, _AD.slice(bd));
    x = Mat.solve(A, b);

    count = 0;
    Mat.forEach(x, (item: any) => {
      rp[count++] = item;
    })

    // re-fit for parameters A - C with those center coordinates
    for (i = 0; i < n; i++) {
      let p = JSON.parse(JSON.stringify(_AD[ptsf_copy + i]));
      p.x -= c.x;
      p.y -= c.y;
      let px = p.x*scale;
      let py = p.y*scale;
      _AD[bd + i] = 1.0;
      _AD[Ad + i * 3] = (px - rp[0]) * (px - rp[0]);
      _AD[Ad + i * 3 + 1] = (py - rp[1]) * (py - rp[1]);
      _AD[Ad + i * 3 + 2] = (px - rp[0]) * (py - rp[1]);
    }
    A = Mat.matrix( n, 3, _AD);
    b = Mat.matrix( n, 1, _AD.slice(bd));
    x = Mat.solve(A, b);

    count = 0;
    Mat.forEach(x, (item: any) => {
      gfp[count++] = item;
    })

    // store angle and radii
    rp[4] = -0.5 * Math.atan2(gfp[2], gfp[1] - gfp[0]); // convert from APP angle usage
    if( Math.abs(gfp[2]) > min_eps )
      t = gfp[2]/Math.sin(-2.0 * rp[4]);
    else // ellipse is rotated by an integer multiple of pi/2
      t = gfp[1] - gfp[0];
    rp[2] = Math.abs(gfp[0] + gfp[1] - t);
    if( rp[2] > min_eps )
      rp[2] = Math.sqrt(2.0 / rp[2]);
    rp[3] = Math.abs(gfp[0] + gfp[1] + t);
    if( rp[3] > min_eps )
      rp[3] = Math.sqrt(2.0 / rp[3]);

    box.center.x = (rp[0] / scale) + c.x;
    box.center.y = (rp[1] / scale) + c.y;
    box.size.width = (rp[2] * 2 / scale);
    box.size.height = (rp[3] * 2 / scale);
    if (box.size.width > box.size.height) {
      let tmp = box.size.width;
      box.size.width = box.size.height;
      box.size.height = tmp;
      box.angle = (90 + rp[4] * 180 / Math.PI);
    }
    if (box.angle < -180)
      box.angle += 360;
    if (box.angle > 360)
      box.angle -= 360;

    return box;
  }

  static fitEllipse(points: Point[]) {
    return MiniOpenCV.fitEllipseNoDirect(points);
  }
}