Closed shadow1runner closed 8 years ago
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))
}
def affineModelCoefficients(posLst: Seq[(Int, Int)]): AffineCoefficients = {
}