Esempio n. 1
0
void DeterministicSLAM::mergeOtherSegments(const SegmentScan &s)
{
    //??QList<Geometry::LineSegment *> outdoorSegments;
    const AlignedVector<UncertainLineSegment> &scan = s.getSegments();
    int i, j;
    /* Slow O(n^2) search for line segments to merge */
    for(i = 0; i < scan.size(); i++) {
        for(j = 0; j < otherSegments.size(); j++) {
            LineSegment &lm = *(otherSegments[j]);
            const UncertainLineSegment &ls = scan[i];
            double angle = linewrap(lm.angle() - ls.angle());

            /* Check if the segments are almost collinear (and long enough) */
            if(lm.length() > 0.1 && ls.length() > 0.1 &&
                    std::fabs(angle) < Config::SLAM::collinearityThreshold) {
                /* Get a new line segment by rotating around the centroid in order to align
                   the two segments */
                LineSegment rotated = Rototranslation(ls.centroid(), angle) * ls;
                const Point &rp1 = rotated.p1();
                const Point &rp2 = rotated.p2();

                /* Check if the point to line distance of one (either one is ok) of the
                   rotated segment is small enough */
                if(lm.distance2(rp1) < SQUARE(Config::SLAM::mergeThreshold)) {
                    /* Check that the projection of at least one the endpoints falls in an
                       acceptable range */
                    double proj1 = lm.project1D(rp1), proj2 = lm.project1D(rp2);
                    double accept_l = - Config::SLAM::mergeThreshold;
                    double accept_u = lm.length() + Config::SLAM::mergeThreshold;

                    /* TODO: Serve che il controllo sulla distanza abbia isolinee circolari,
                             non rettangolari come ora */
                    if((proj1 >= accept_l && proj1 <= accept_u) ||
                            (proj2 >= accept_l && proj2 <= accept_u)) {

                        /* Merge the line segments */
                        lm.mergeInPlace(ls);
                        //???outdoorSegments.append(new LineSegment(lm));
                        //lm.mergeInPlaceProjection(ls);
                        break;
                    }
                }
            }
        }

        /* No segments were merged, add the last one to the map */
        if(j == otherSegments.size() ){//&& scan[i].length() > SM_MINIMUM_SEGMENT_LENGTH) {
            LineSegment *s = new LineSegment(scan[i]);
            otherSegments.append(s);
            //??outdoorSegments.append(new LineSegment(scan[i]));
        }
    }
    /*if(outdoor){
        segments=outdoorSegments;
    }*/
}
Esempio n. 2
0
 inline double squared_length(const LineSegment& s)
 {
   return (s.p2()-s.p1()).squaredNorm();
 }
Esempio n. 3
0
 inline double length(const LineSegment& s)
 {
   return (s.p2()-s.p1()).norm();
 }
Esempio n. 4
0
 inline Vector2d dir(const LineSegment& s)
 {
   return s.p2() - s.p1();
 }