Frequency Modulated Continuous Wave Radar And Video Fusion For Simultaneous Localization And Mapping