예제 #1
0
파일: Surface.cpp 프로젝트: todayman/Cinder
	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() );
	}
예제 #2
0
파일: Threshold.cpp 프로젝트: Ahbee/Cinder
void threshold( const SurfaceT<T> &surface, T value, SurfaceT<T> *dstSurface )
{
	thresholdImpl( surface, value, surface.getBounds(), ivec2(), dstSurface );
}
예제 #3
0
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);
	}
}