Fix some bugs

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -105,7 +105,7 @@ namespace Assets.StreetLight.Scripts
             {
                 var x1 = correspondence.WorldPosition;
                 var x2 = correspondence.UnityPosition;
-                var x1Transformed = TransformPoint(x1, homography);
+                var x1Transformed = TransformPoint(x1, currentH);
                 var x2Transformed = TransformPoint(x2, inverseH);
                 distances.Add(Math.Pow((x1Transformed - x2).L1Norm(), 2) + Math.Pow((x1 - x2Transformed).L1Norm(), 2));
             }
@@ -115,7 +115,7 @@ namespace Assets.StreetLight.Scripts
 
         private Vector<double> TransformPoint(Vector<double> point, Matrix<double> homography)
         {
-            if (point.Count != 3 || homography.RowCount != 3 || homography.ColumnCount != 3)
+            if (point.Count != 2 || homography.RowCount != 3 || homography.ColumnCount != 3)
             {
                 throw new ArgumentException();
             }