                      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;
 

      } 
    }
  }
  // if (loopIndex == 6) printf("%d B: %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, B[0][0], B[0][1], B[0][2], B[0][3], B[1][0], B[1][1], B[1][2], B[1][3], B[2][0], B[2][1], B[2][2], B[2][3], B[3][0], B[3][1], B[3][2], B[3][3]);

  // free(R12);
  // free(R21);
  delete [] R22_1;
  delete [] R22;
  delete [] x;

      numInliers++;
      d_inliers[loopIndex * numPts + i] = 1;
    } 
  }

  // Technically we shouldn't overwrite; we're also writing numLoops-major
