ICP working, added OPENCV viz