Implement affine motion model for divergence (mainly for comparison reasons)

[shadow1runner]

def affineModelCoefficients(posLst: Seq[(Int, Int)]): AffineCoefficients = {

var A: DenseMatrix[Double] = null //DenseMatrix.zeros[Double](vxxKF.stateVectorLength, 8)
val b = DenseVector.zeros[Double](posLst.length * 2)

// Construct the b vector
for (((x, y), i) ← posLst.zipWithIndex) {
  val vec = this(x, y)
  b(i * 2) = vec.x
  b((i * 2) + 1) = vec.y
}

// Construct system of linear equations

val xCenter = size.width / 2
val yCenter = size.height / 2

for ((x, y) ← posLst) {
  val xPart = DenseMatrix(1d, (x - xCenter), (y - yCenter), 0, 0, 0)
  val yPart = DenseMatrix(0, 0, 0, 1d, (x - xCenter), (y - yCenter))
  if (A == null) {
    A = xPart
    A = DenseMatrix.horzcat(A, yPart)
  } else {
    A = DenseMatrix.horzcat(A, xPart, yPart)
  }
}

A = A.t // Since we constructed the matrix columnwise

// TODO: Use QR Decomposition for better numerical accuracy
// http://en.wikipedia.org/wiki/QR_factorization
AffineCoefficients((inv(A.t * A) * A.t) * VectorFloatToDouble(b))

}