void SegmentScan::split(const PointScan &pscan, const IndexSet &idx) { /* No segment to fit */ if(idx.size() < 2) return; /* Fit line segment between first and last point (only do least squares fitting when splitting has ended) */ LineSegment segment(pscan[idx.indexBegin()], pscan[idx.indexEnd() - 1]); #if 0 /* Minimal segment */ if(idx.size() == 2) { const UncertainLineSegment usegment = fittingFunction(pscan, idx); const LineSegmentTuple tuple = { usegment, idx }; clusterSupport.append(tuple); return; } #endif /* Find most distant point from the line */ int maxDistIdx; double maxDist = findMaximumDistance(pscan, idx, segment, &maxDistIdx); /* Split recursively if necessary, otherwise add the segment to the list */ if(maxDist > SQUARE(Config::SLAM::splitThreshold)) { //ldbg << "split 1: " << idx.firstHalfSplit(maxDistIdx) << endl; //ldbg << "split 2: " << idx.secondHalfSplit(maxDistIdx) << endl; split(pscan, idx.firstHalfSplit(maxDistIdx)); split(pscan, idx.secondHalfSplit(maxDistIdx)); } else { const UncertainLineSegment usegment = fittingFunction(pscan, idx); const LineSegmentTuple tuple(usegment, idx); clusterSupport.append(tuple); } }