                      0, 1, 0, -y_centroid[1],
                      0, 0, 1, -y_centroid[2],
                      0, 0, 0, 1 };
-  // printf("%d T1: %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n", loopIndex, T1[0][0], T1[0][1], T1[0][2], T1[0][3], T1[1][0], T1[1][1], T1[1][2], T1[1][3], T1[2][0], T1[2][1], T1[2][2], T1[2][3], T1[3][0], T1[3][1], T1[3][2], T1[3][3]);
 
   float T2[4][4] = { rot[0], rot[1], rot[2], 0,
                      rot[3], rot[4], rot[5], 0,
                      rot[6], rot[7], rot[8], 0,
                      0, 0, 0, 1 };
-  // printf("%d T2: %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n", loopIndex, T2[0][0], T2[0][1], T2[0][2], T2[0][3], T2[1][0], T2[1][1], T2[1][2], T2[1][3], T2[2][0], T2[2][1], T2[2][2], T2[2][3], T2[3][0], T2[3][1], T2[3][2], T2[3][3]);
 
   float T3[4][4] = { 1, 0, 0, x_centroid[0],
                      0, 1, 0, x_centroid[1],
                      0, 0, 1, x_centroid[2],
                      0, 0, 0, 1 };
-  // printf("%d T3: %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n", loopIndex, T3[0][0], T3[0][1], T3[0][2], T3[0][3], T3[1][0], T3[1][1], T3[1][2], T3[1][3], T3[2][0], T3[2][1], T3[2][2], T3[2][3], T3[3][0], T3[3][1], T3[3][2], T3[3][3]);
-
-
-  // free(x_centroid);
-  // free(y_centroid);
+
   delete [] x_centroid;
   delete [] y_centroid;
 

  // Col 3: Negative Col 4 from Matlab
  // Col 4: Negative Col 3 from Matlab
  dsvd(B, 4, 4, S, V);
  // if (loopIndex == 6) printf("%d V: %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n     %0.4f %0.4f %0.4f %0.4f\n", loopIndex, V[0][0], V[0][1], V[0][2], V[0][3], V[1][0], V[1][1], V[1][2], V[1][3], V[2][0], V[2][1], V[2][2], V[2][3], V[3][0], V[3][1], V[3][2], V[3][3]);
  // if (loopIndex == 6) printf("%d S: %0.4f %0.4f %0.4f %0.4f\n", loopIndex, S[0], S[1], S[2], S[3]);

  // TODO: Getting quaternion is different
  int ind = 0;
  float minsingularvalue = S[0];
  for (int i = 1; i < 4; i++) {


  // Loop through all the points (3 because we're doing three-point
  // correspondance) to get coordinates
  for (int i = 0; i < pointCount; i++) {
    // Get point index
    int pointIndex = indices[i];

    // Get the three random points (ref coords)
    x[i * dim + 0] = d_coord[6 * pointIndex + 0];
    x[i * dim + 1] = d_coord[6 * pointIndex + 1];
    x[i * dim + 2] = d_coord[6 * pointIndex + 2];

    // Get the three random points (mov coords)
    y[i * dim + 0] = d_coord[6 * pointIndex + 3];
    y[i * dim + 1] = d_coord[6 * pointIndex + 4];
    y[i * dim + 2] = d_coord[6 * pointIndex + 5];
  }

  // Compute the centroid of the three random points by taking the average of
  // their coordinates
  float *x_centroid = new float[dim]();
  float *y_centroid = new float[dim]();

  for (int i = 0; i < pointCount; i++) {
    x_centroid[0] += x[i * dim + 0];
    x_centroid[1] += x[i * dim + 1];
