How to complete matrix entries of a minimum cost function to remove outliers from an image?

See more:

C#

processing

image

I am trying to align an image B with another image A, I have a method that should return the transformation that should align B to A with the minimum cost possible. I have gotten the derives to use in the method from a research paper that was published on research gate. When I call the method, the transformation returned places image B on top of image A instead of aligning them with the minimum cost. The method takes two parameters which happen to be List objects containing the Points of image A and image B. Its supposed to return the best transformation with the minimum cost that aligns B to A.
I suspect that the matrix completion entries I used inside this loop is the problem, please help

What I have tried:

 public static Transformation ComputeTransformation(List<Point> shp1, List<Point> shp2) {
           PCALib.Matrix A = new PCALib.Matrix(4,4);
           PCALib.Matrix B = new PCALib.Matrix(4, 1);
           //iterate through the points stored in the first shape
           for(int i = 0; i < shp1.Count; i++)
           {
               A[0,0] +=2*shp2[i].X*shp2[i].X+2*shp2[i].Y*shp2[i].Y;
               //completing the remaining entries for matrix A
               //it is important to note that matrix A is 4 by 4
               A[0, 1] += 0;
               A[0, 2] += 2 * shp2[i].X;
               A[0, 3] += 2 * shp2[i].Y;
               A[1, 0] += 0;
               A[1, 1] += 2 * shp2[i].Y * shp2[i].Y+2*shp2[i].X*shp2[i].X;
               A[1, 2] += 2 * shp2[i].Y;
               A[1, 3] += -2 * shp2[i].X;
               A[2, 0] += 2 * shp2[i].X;
               A[2, 1] += 2 * shp2[i].Y;
               A[2, 2] += 2;
               A[2, 3] += 0;
               A[3, 0] += 2 * shp2[i].Y;
               A[3, 1] += -2 * shp2[i].X;
               A[3, 2] += 0;
               A[3, 3] += 2;
               //completing the remaining entries for matrix B
               //the dimensions of Matrix B is 4 by one
               B[0, 0] += 2 * shp1[i].X * shp2[i].X + 2 * shp1[i].Y * shp2[i].Y;
               B[1, 0] += 2 * shp1[i].X * shp2[i].Y - 2 * shp2[i].X * shp1[i].Y;
               B[2, 0] += 2 * shp1[i].X;
               B[3, 0] += 2 * shp1[i].Y;


           }


public class Transformation
   {
       public double A { get; set; }
       public double B { get; set; }
       public double T1 { get; set; }
       public double T2 { get; set; }
   }