ImageSourceSurface( const SurfaceT<T> &surface )
: ImageSource()
{
mWidth = surface.getWidth();
mHeight = surface.getHeight();
setColorModel( ImageIo::CM_RGB );
setChannelOrder( ImageIo::ChannelOrder( surface.getChannelOrder().getImageIoChannelOrder() ) );
if( boost::is_same<T,uint8_t>::value ) {
setDataType( ImageIo::UINT8 );
mSurface8u = *reinterpret_cast<const Surface8u*>( &surface ); // register reference to 'surface'
}
else if( boost::is_same<T,uint16_t>::value ) {
setDataType( ImageIo::UINT16 );
mSurface16u = *reinterpret_cast<const Surface16u*>( &surface ); // register reference to 'surface'
}
else if( boost::is_same<T,float>::value ) {
setDataType( ImageIo::FLOAT32 );
mSurface32f = *reinterpret_cast<const Surface32f*>( &surface ); // register reference to 'surface'
}
else
throw; // this surface seems to be a type we've never met
mRowBytes = surface.getRowBytes();
mData = reinterpret_cast<const uint8_t*>( surface.getData() );
}
void threshold( const SurfaceT<T> &surface, T value, SurfaceT<T> *dstSurface )
{
thresholdImpl( surface, value, surface.getBounds(), ivec2(), dstSurface );
}
void edgeDetectSobel( const SurfaceT<T> &srcSurface, SurfaceT<T> *dstSuface )
{
edgeDetectSobel( srcSurface, srcSurface.getBounds(), Vec2i::zero(), dstSuface );
}
/* 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;
}
/* 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;
}
/*
* 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);
}
}