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();
}
}
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();
}
}
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();
}
}
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();
}
}
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();
}
}
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();
}
}
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();
}
}
