To address problem that the ORB-SLAM2 algorithm is prone to mismatching and cannot build a dense map during feature matching, the GMS algorithm is introduced to improve the mismatching problem in the ORB-SLAM2 algorithm and add a dense map thread. First, an image pyramid is established, and a grid division is performed on each layer of the image pyramid to extract feature points. A four-tree strategy is introduced for feature point selection in each grid, resulting in a uniform distribution of feature points. Second, the GMS algorithm is introduced in the feature matching stage to eliminate false matches. Finally, the dense point cloud map is built based on the pose estimation and key frames. Through the experimental verification on TUM data set, the results show that the matching number of the improved algorithm is 7.82% higher than that of the original ORB-SLAM2 algorithm, and the matching time is reduced by 8.53%. The improved algorithm is applied to the automatic navigation and obstacle avoidance of mobile robot, which can improve the reliability and operation efficiency of the system.