/*
* Returns the smallest distance between two surfaces (limited to their endpoints).
*/
double distBetweenSurfs(const SurfaceT & s1, const SurfaceT & s2) {
// Distance s1.P1 from SurfaceT s2
PointXY test = nearestPointOnSurf(s2, s1.getP1(), true);
double closestDistSq = test.distFromSq(s1.getP1());
// Distance s1.P2 from SurfaceT s2
test = nearestPointOnSurf(s2, s1.getP2(), true);
double testDistSq = test.distFromSq(s1.getP2());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
// Distance SurfaceT s1 from s2.P1
test = nearestPointOnSurf(s1, s2.getP1(), true);
testDistSq = test.distFromSq(s2.getP1());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
// Distance SurfaceT s1 from s2.P2
test = nearestPointOnSurf(s1, s2.getP2(), true);
testDistSq = test.distFromSq(s2.getP2());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
return sqrt(closestDistSq);
}
PointXY nearestPointOnSurf(const SurfaceT & surf, const PointXY & other, bool limitToEndpoints) {
/* Find point P so that P = P1 + u(P2-P1) (P is on the line P1,P2)
* Also, (P3-P) dot (P2-P1) = 0. (P3,P is orthogonal to P1,P2)
* Substituted: (P3-P1 - u(P2-P1)) dot (P2-P1) = 0
* Solve for u!
*/
double x1 = surf.getX1(), y1 = surf.getY1();
double x2 = surf.getX2(), y2 = surf.getY2();
double x3 = other.getX(), y3 = other.getY();
double u = ((x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1))
/ (surf.getLength() * surf.getLength()); // Note: abs(P2-P1) == length
// Now: p is on the infinite line described by p1 and p2. But is it on the surface segment?
if (limitToEndpoints && (u < 0.0 || u > 1.0)) {
// p is not within the surface bounds. Return the nearest end point instead!
if (surf.getP1().distFromSq(other) < surf.getP2().distFromSq(other))
return surf.getP1();
else
return surf.getP2();
} else {
// Substitute to find p
double xP = x1 + u * (x2 - x1);
double yP = y1 + u * (y2 - y1);
return PointXY(xP, yP);
}
}
/* Extends the surface (tobeextended) to have the length of (extensionReference).
* Keeps the ID of tobeextended, but changes occlusion information.
* If extendFromPoint1 is true, then P1 will be fixed and P2 be changed.
*/
const SurfaceT extendSurfaceT(const SurfaceT & toBeExtended,
const SurfaceT & extensionReference,
bool extendFromPoint1) {
if (!toBeExtended.isValid()) {
cerr << "WARNING: Cannot extend surface: No surface to extend!" << endl;
return SurfaceT::INVALID;
}
double oldLength = toBeExtended.getLength();
double newLength = extensionReference.getLength();
PointXY newP1, newP2;
// Extend the surface along its direction vector
if (extendFromPoint1) {
double dirX = (toBeExtended.getX2() - toBeExtended.getX1()) / oldLength;
double dirY = (toBeExtended.getY2() - toBeExtended.getY1()) / oldLength;
newP1 = toBeExtended.getP1();
newP2 = PointXY(newP1.getX() + dirX * newLength,
newP1.getY() + dirY * newLength);
} else {
double dirX = (toBeExtended.getX1() - toBeExtended.getX2()) / oldLength;
double dirY = (toBeExtended.getY1() - toBeExtended.getY2()) / oldLength;
newP2 = toBeExtended.getP2();
newP1 = PointXY(newP2.getX() + dirX * newLength,
newP2.getY() + dirY * newLength);
}
// Change the points and keep the ID
SurfaceT extendedSurf(newP1, newP2, toBeExtended.getId());
// If the (longer) surface has both occluding edges, then the extended one will have too.
if (extensionReference.isP1Occluding() && extensionReference.isP2Occluding()) {
extendedSurf.setP1Occluding(true);
extendedSurf.setP2Occluding(true);
} else {
// Otherwise keep old occlusion info (usually one occluding edge out of two)
extendedSurf.setP1Occluding(toBeExtended.isP1Occluding());
extendedSurf.setP2Occluding(toBeExtended.isP2Occluding());
}
extendedSurf.setBoundarySurf(toBeExtended.isBoundarySurf());
return extendedSurf;
}
/* Gets the angle and distance between newSurf and referenceNew, and applies them to referenceOld.
* The result is a surface in the old coordinate space.
* Used for updating the MFIS with new surfaces.
*/
SurfaceT distanceAngleTransform(const SurfaceT & newSurf,
const SurfaceT & referenceNew,
const SurfaceT & referenceOld,
SurfaceTMatch::MatchingSurfaceTEndpoints referencePoints) {
// Decide which point to use as reference, and whether the surfaces are facing in the same direction
PointXY refPointNew;
PointXY refPointOld;
bool facingOppositeDirection = false;
switch (referencePoints) {
case SurfaceTMatch::OLD1_NEW1_AND_OLD2_NEW2:
case SurfaceTMatch::OLD1_NEW1_ONLY:
refPointNew = referenceNew.getP1();
refPointOld = referenceOld.getP1();
break;
case SurfaceTMatch::OLD2_NEW2_ONLY:
refPointNew = referenceNew.getP2();
refPointOld = referenceOld.getP2();
break;
case SurfaceTMatch::OLD1_NEW2_AND_OLD2_NEW1:
case SurfaceTMatch::OLD1_NEW2_ONLY:
refPointNew = referenceNew.getP2();
refPointOld = referenceOld.getP1();
facingOppositeDirection = true;
break;
case SurfaceTMatch::OLD2_NEW1_ONLY:
refPointNew = referenceNew.getP1();
refPointOld = referenceOld.getP2();
facingOppositeDirection = true;
break;
default:
cerr << "WARNING: Reference surface endpoints don't match!";
return SurfaceT::INVALID;
}
/* For each new surface, get the distance and angle relative to the reference surface (for both points).
* The distance is relative to P1 of the reference,
* the angle is relative to the direction (p1->p2) of the reference.
*/
double distanceP1 = newSurf.getP1().distFrom(refPointNew);
double distanceP2 = newSurf.getP2().distFrom(refPointNew);
double angleP1 = referenceNew.getAngleDiffTo(SurfaceT(refPointNew, newSurf.getP1()));
double angleP2 = referenceNew.getAngleDiffTo(SurfaceT(refPointNew, newSurf.getP2()));
// Construct new points, using angle and distance on the reference in the MFIS
// Get the (normalized) direction of the MFIS reference surface
double refDirLength = referenceOld.getLength();
double refDirX = (referenceOld.getX2() - referenceOld.getX1()) / refDirLength;
double refDirY = (referenceOld.getY2() - referenceOld.getY1()) / refDirLength;
// If the reference surfaces are facing opposite each other, invert the direction
if (facingOppositeDirection) {
refDirX *= -1;
refDirY *= -1;
}
// Rotate the direction by the angle
double dirXP1 = cos(deg2rad(angleP1)) * refDirX - sin(deg2rad(angleP1)) * refDirY;
double dirYP1 = sin(deg2rad(angleP1)) * refDirX + cos(deg2rad(angleP1)) * refDirY;
double dirXP2 = cos(deg2rad(angleP2)) * refDirX - sin(deg2rad(angleP2)) * refDirY;
double dirYP2 = sin(deg2rad(angleP2)) * refDirX + cos(deg2rad(angleP2)) * refDirY;
// Move the distance along the new direction
double xP1MFIS = refPointOld.getX() + distanceP1 * dirXP1;
double yP1MFIS = refPointOld.getY() + distanceP1 * dirYP1;
double xP2MFIS = refPointOld.getX() + distanceP2 * dirXP2;
double yP2MFIS = refPointOld.getY() + distanceP2 * dirYP2;
// Construct a new surface for the MFIS and copy the ID
SurfaceT transformedSurfaceT(PointXY(xP1MFIS, yP1MFIS),
PointXY(xP2MFIS, yP2MFIS),
newSurf.getId());
// Copy the occlusion information
transformedSurfaceT.setP1Occluding(newSurf.isP1Occluding());
transformedSurfaceT.setP2Occluding(newSurf.isP2Occluding());
transformedSurfaceT.setBoundarySurf(newSurf.isBoundarySurf());
return transformedSurfaceT;
}
/*
* Marks boundary surfaces in a vector (single view).
* The surfaces MUST be ordered by ID, and also geometrically (angle in respect to origin)!
* => It works only with surfaces directly from the laser scan.
*/
void markBoundarySurfs(vector<SurfaceT> & surfaces, bool plotDebugImg)
{
const PointXY robotPos(0, 0);
if(surfaces.size() < 1)
{
cerr << "WARNING: No surfaces found!" << endl;
return;
}
// Maintain and modify a linked list of boundary surfaces
list<SurfaceT> bSurfs;
bSurfs.insert(bSurfs.end(), surfaces.begin(), surfaces.end());
/* This stack remembers "inward" movements, such as from occluded edges.
* Once we find an "outward" movement, we can shortcut it by connecting the outer surfaces
* (and removing the inner ones).
*/
stack<list<SurfaceT>::iterator> inwardsIndex;
for(list<SurfaceT>::iterator it = ++bSurfs.begin(); it != bSurfs.end(); ++it)
{
list<SurfaceT>::iterator prevIt = it;
--prevIt;
// Moving inwards
if(prevIt->getP2().distFrom(robotPos) > it->getP1().distFrom(robotPos) + 500)
{
inwardsIndex.push(it);
}
// Moving outwards
if(prevIt->getP2().distFrom(robotPos) < it->getP1().distFrom(robotPos) - 500)
{
// Remove "inside" surfs
while(inwardsIndex.size() > 0)
{
list<SurfaceT>::iterator eraseStart = inwardsIndex.top();
/* Get the angle difference between the inward/outward connections.
* If it's too big, don't shortcut across the whole view!
*/
double angleDiff = SurfaceT(eraseStart->getP2(), robotPos).getAngleDiffTo(SurfaceT(it->getP1(), robotPos));
if(normAngleDiff(angleDiff) > 45)
break;
// Angle is small, remove the inner surfaces
inwardsIndex.pop();
bSurfs.erase(eraseStart, it);
}
}
}
// We have the boundary surfaces, now mark them in the original vector
vector<SurfaceT>::iterator surfIt = surfaces.begin();
for(list<SurfaceT>::const_iterator bSurfIt = bSurfs.begin(); bSurfIt != bSurfs.end(); ++bSurfIt)
{
while(surfIt->getId() < bSurfIt->getId())
++surfIt;
if(surfIt->getId() != bSurfIt->getId())
{
cerr << "ERROR: could not mark boundary surfaces! There is a problem in surface ID order!" << endl;
return;
}
surfIt->setBoundarySurf(true);
}
if(plotDebugImg)
{
// For plotting: Connect the surfaces endpoint-to-endpoint.
vector<SurfaceT> connections;
connections.push_back(SurfaceT(PointXY(0,0), bSurfs.front().getP1()));
SurfaceT prevSurf = bSurfs.front();
for(list<SurfaceT>::const_iterator it = ++bSurfs.begin(); it != bSurfs.end(); ++it)
{
SurfaceT connection(prevSurf.getP2(), it->getP1());
if(connection.getLength() > 50)
connections.push_back(connection);
prevSurf = *it;
}
connections.push_back(SurfaceT(bSurfs.back().getP2(), PointXY(0,0)));
vector<vector<SurfaceT> > plotVector;
plotVector.push_back(surfaces);
plotVector.push_back(connections);
vector<SurfaceT> bSurfsV;
bSurfsV.insert(bSurfsV.end(), bSurfs.begin(), bSurfs.end());
plotVector.push_back(bSurfsV);
cout << "Plotting boundary.png ..." << endl;
//plotSurfaces("boundary.png", plotVector);
}
}