Environment-Detection-and-Mapping Algorithm for Autonomous Driving in Rural or Off-Road Environment