Example #1
0
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);
    }

}