예제 #1
0
void CalibratorBlock::run()
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent(1000);
        if (event.isNull())
            continue;

        FrameVectorVectorConstPtr data = dynamic_Ptr_cast<FrameVectorVector>(event.data);
        if (!data)
            continue;

        ntk_dbg(1) << "[CalibratorBlock] New image received";

        {
            QMutexLocker _(&m_calibrator_mutex);
            switch (m_algo)
            {
            case ICP:
                calibrateWithICP(data);
                break;
            case Chessboard:
                calibrateWithChessboard(data);
                break;
            default:
                ;
            };
        }

        ntk_dbg(1) << "[CalibratorBlock] Calibration finished.";
        reportNewEventProcessed();
    }
}
예제 #2
0
void RecorderBlock::run() //FB011
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent(200);
        if (event.isNull())
            continue;

        RGBDImagePtr image = dynamic_Ptr_cast<RGBDImage>(event.data);
        if (image)
        {
            QMutexLocker _(&m_recorder_mutex);
            m_recorder->saveCurrentFrame(*image);
        }
		else
		{
			FrameVectorPtr frames = dynamic_Ptr_cast<FrameVector>(event.data);
			if (frames)
			{
	     		QMutexLocker _(&m_recorder_mutex);
				m_recorder->saveCurrentFrames(frames->images);
			}
		}
        
        reportNewEventProcessed();
    }
}
예제 #3
0
void FrameSynchronizerBlock::run()
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent(1000);
        if (event.isNull())
            continue;

        RGBDImagePtr image = dynamic_Ptr_cast<RGBDImage>(event.data);
        if (!image)
            continue;

        ntk_dbg(2) << "[FrameSync] New image received";

        // Another device has to be waited for?
        if (m_devices_serial_to_wait.find(image->cameraSerial()) == m_devices_serial_to_wait.end())
            continue;

        // Update the image.
        m_updated_images[image->cameraSerial()] = image;

        // Not all images received? Keep waiting for more events.
        if (m_updated_images.size() < m_devices_serial_to_wait.size())
            continue;

        // Everything received. Can generate a new event.
        FrameVectorPtr new_data (new FrameVector);
        foreach_const_it(it, m_updated_images, image_map_type)
            new_data->images.push_back(it->second);
        broadcastEvent(new_data);

        ntk_dbg(2) << "[FrameSync] All images updated, sending new event!";

		//ntk::sleep(500); //FB011
		//Force FPS
		if (m_prevFrameStartTime > 100000000) {
			m_prevFrameStartTime = ntk::Time::getMillisecondCounter();
		}
        uint64 frameDuration = (ntk::Time::getMillisecondCounter() - m_prevFrameStartTime)/1000;
        int sleepTime = 1000/_FPS - frameDuration;
        if (sleepTime > 0.0f) {
            ntk::sleep(sleepTime);
        }
		//std::cerr << "m_prevFrameStartTime: " << m_prevFrameStartTime << std::endl;
        m_prevFrameStartTime = ntk::Time::getMillisecondCounter();
        
        // Reset images.
        m_updated_images.clear();

        reportNewEventProcessed();
    }
}
예제 #4
0
void RecorderBlock::run()
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent();
        if (event.isNull())
            continue;

        RGBDImagePtr image = dynamic_Ptr_cast<RGBDImage>(event.data);
        ntk_assert(image, "No image!");

        {
            QMutexLocker _(&m_recorder_mutex);
            m_recorder->saveCurrentFrame(*image);
        }

        reportNewEventProcessed();
    }
}
예제 #5
0
void RGBDProcessorBlock::run()
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent(200);
        if (event.isNull())
            continue;

        FrameVectorPtr data = dynamic_Ptr_cast<FrameVector>(event.data);
        ntk_assert(data, "No images!");

        for (size_t i = 0; i < data->images.size(); ++i)
        {
            QMutexLocker _(&m_processor_mutex);
            m_processor.processImage(*data->images[i]);
        }
        broadcastEvent(data);
        // FIXME: make this configurable.
        //ntk::sleep(50); //FB011

        reportNewEventProcessed();
    }
}
예제 #6
0
void MeshGeneratorBlock::run()
{
    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent(1000);
        if (event.isNull())
            continue;

        FrameVectorConstPtr data = dynamic_Ptr_cast<FrameVector>(event.data);
        if (!data)
            continue;

        ntk_dbg(2) << "[MeshGeneratorBlock] New image received";

        MeshVectorPtr new_event (new MeshVector());
        new_event->meshes.resize(data->images.size());
        new_event->camera_serials.resize(data->images.size());

        {
            QMutexLocker _(&m_generator_mutex);
            for (size_t i = 0; i < data->images.size(); ++i)
            {
                m_mesh_generator.generate(*data->images[i]);
                MeshPtr mesh (new Mesh);
                *mesh = m_mesh_generator.mesh();
                new_event->meshes[i] = mesh;
                new_event->camera_serials[i] = data->images[i]->cameraSerial();
            }
        }

        broadcastEvent(new_event);

        ntk_dbg(2) << "[MeshGeneratorBlock] All meshes created, sending new event!";

        reportNewEventProcessed();
    }
}
예제 #7
0
void FrameSynchronizerBlock::run()
{
    // m_wait_until_only_one_has_depth = true;

    while (!threadShouldExit())
    {
        EventListener::Event event = waitForNewEvent();
        if (event.isNull())
            continue;

        RGBDImagePtr image = dynamic_Ptr_cast<RGBDImage>(event.data);
        if (!image)
            continue;

        ntk_dbg(2) << "[FrameSync] New image received";

        // Another device has to be waited for?
        if (m_devices_serial_to_wait.find(image->cameraSerial()) == m_devices_serial_to_wait.end())
            continue;

        QMutexLocker _(&mutex);

        if (m_wait_until_only_one_has_depth)
        {
            if (image->hasEmptyRawDepthImage())
            {
                images_without_depth_map_type::iterator it;
                it = m_images_without_depth.find(image->cameraSerial());
                if (it == m_images_without_depth.end() || it->second == 0)
                    m_images_without_depth[image->cameraSerial()] = ntk::Time::getMillisecondCounter();
                continue;
            }

            m_images_without_depth[image->cameraSerial()] = 0;
            if (!otherImagesHaveNoDepth(image->cameraSerial()))
                continue;
        }

        // Require an alternative activation of each camera.
        if (m_wait_until_only_one_has_depth
                && m_last_added_image == image->cameraSerial())
            continue;

        // Update the image.
        m_updated_images[image->cameraSerial()] = image;
        m_last_added_image = image->cameraSerial();
        ntk_dbg(2) << image->cameraSerial() << " validated";

        // Not all images received? Keep waiting for more events.
        if (m_updated_images.size() < m_devices_serial_to_wait.size())
            continue;

        // Everything received. Can generate a new event.
        FrameVectorPtr new_data (new FrameVector);
        new_data->images.resize(m_updated_images.size());
        int i = 0;
        foreach_const_it(it, m_updated_images, image_map_type)
            new_data->images[i++] = it->second;
        broadcastEvent(new_data);

        ntk_dbg(2) << "[FrameSync] All images updated, sending new event!";

        // Reset images.
        m_updated_images.clear();

        if (m_wait_until_only_one_has_depth)
            m_images_without_depth.clear();

        reportNewEventProcessed();
    }
}