Added working ICP for camera extrinsic calibration