A Novel Framework for Data Registration and Data Fusion in Presence of Multi-modal Sensors