Finished ICP calibration and added transform between cameras. Need to check correctness