Line segment algorithm for LRF data

I want to cluster the LRF scanned data into the clusters by line segmentation method. environment is consisting of walls or flat surfaces only. The output of the algorithm should be:

- All the segmented lines - Starting and end points of each segment - length of the segments - Finally Best fitting line segment using LSQ

If anyone has developed this algorithm for simulation purpose in MATLAB, forward me.

[chrt gnnt]

Were you able to find an answer to this question? I am using a Hokuyo URG-04LX LRF..