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;
}*/
}
inline double squared_length(const LineSegment& s)
{
return (s.p2()-s.p1()).squaredNorm();
}
inline double length(const LineSegment& s)
{
return (s.p2()-s.p1()).norm();
}
inline Vector2d dir(const LineSegment& s)
{
return s.p2() - s.p1();
}