LOCALIZATION AND MAPPING ALGORITHM BASED ON LIDAR-IMU-CAMERA FUSION