In order to enable the robot to achieve relatively clear map construction and accurate self-positioning in a dim and fuzzy underwater environment, the front-end was optimized based on the Rtabmap algorithm. The histogram equalization algorithm was used to preprocess the underwater images acquired by the stereo camera, and the brightness of the image and the clarity of the detail were improved. The image feature points were extracted and matched by using the ORB algorithm, and the mismatched point pairs were eliminated by using the RANSAC algorithm, and the accuracy of the camera position estimation was improved. The experimental results show that when the histogram equalization algorithm is used to deal with the underwater blurred images, the feature matching point pairs increases obviously.The optimized Rtab-map algorithm was tested based on the public data set, and the obtained multiple errors were significantly reduced. The experiment was carried out under the sink condition in laboratory. It is verified that the quality of the 3D point cloud images obtained by using the optimized algorithm is better