This paper describes new methods to extract surface features and then group them together. This is required for a planar surface fitting approach that uses both range and vision information to build 3D surface maps for a wall climbing robot. A modified incremental 2D line segmentation approach is presented along with the use of image lines for clustering to improve the performance. A method for verifying the linearity of the line segments was also presented to filter out non-linear line segments so that the planar assumption is not violated during surface fitting. An approach to group these features together was provided based on the physical surfaces that generated them. Both experimental and simulated data sets were used to validate these methods. The results showed that the proposed modifications improved the robustness of the line segmentation by minimising the rate of false positives and the non-linear line segments were successfully filtered out. The feature grouping was performed conservatively and a very low rate of false groupings was seen as a result. These all combine to improve the plane fitting accuracy as the major cause of fitting error is false positive segmentations.