Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Branch: master
Fetching contributors…

Cannot retrieve contributors at this time

302 lines (256 sloc) 8.651 kB
/**
* This file is part of the nestk library.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Author: Nicolas Burrus <nicolas.burrus@uc3m.es>, (C) 2010
*/
#include "GuiController.h"
#include "ui_View3dWindow.h"
#include "ui_RawImagesWindow.h"
#include "View3dWindow.h"
#include "FiltersWindow.h"
#include "RawImagesWindow.h"
#include <ntk/ntk.h>
#include <ntk/camera/rgbd_frame_recorder.h>
#include <ntk/mesh/mesh_generator.h>
#include <ntk/geometry/relative_pose_estimator_icp.h>
#include <QMainWindow>
#include <QImage>
#include <QApplication>
#include <fstream>
using namespace cv;
using namespace ntk;
class ObjectDetector;
GuiController :: GuiController(ntk::RGBDGrabber& producer,
ntk::RGBDProcessor& processor)
: m_grabber(producer),
m_processor(processor),
m_frame_recorder(0),
m_mesh_generator(0),
m_last_tick(cv::getTickCount()),
m_frame_counter(0),
m_frame_rate(0),
m_view3d_window_grabber("/tmp/demo3d/view3d"),
m_raw_window_grabber("/tmp/demo3d/raw"),
m_screen_capture_mode(false),
m_grab_frames(false),
m_paused(false),
m_process_one_frame(false),
m_active_device(0),
m_merge_views(true)
{
m_raw_images_window = new RawImagesWindow(*this);
m_view3d_window = new View3DWindow(*this);
m_filters_window = new FiltersWindow(*this);
m_raw_images_window->show();
}
GuiController :: ~GuiController()
{
delete m_raw_images_window;
delete m_view3d_window;
delete m_filters_window;
}
void GuiController :: quit()
{
m_grabber.stop();
m_grabber.disconnectFromDevice();
QApplication::quit();
}
void GuiController :: setFrameRecorder(ntk::RGBDFrameRecorder& frame_recorder)
{
m_frame_recorder = &frame_recorder;
m_raw_images_window->ui->outputDirText->setText(m_frame_recorder->directory().path());
}
void GuiController :: setMeshGenerator(MeshGenerator& generator)
{
m_mesh_generator = &generator;
m_raw_images_window->ui->action_3D_View->setEnabled(true);
}
static QImage toNormalizedQImage(const cv::Mat1f& m)
{
cv::Mat1b norm;
cv::normalize(m, norm, 0, 255, NORM_MINMAX, 0);
QImage qim (norm.cols, norm.rows, QImage::Format_RGB32);
for (int r = 0; r < norm.rows; ++r)
for (int c = 0; c < norm.cols; ++c)
{
int v = norm(r,c);
qim.setPixel(c,r, qRgb(v,v,v));
}
return qim;
}
static QImage toQImage(const cv::Mat3b& m)
{
QImage qim (m.cols, m.rows, QImage::Format_RGB32);
for (int r = 0; r < m.rows; ++r)
{
QRgb* ptr = (QRgb*) qim.scanLine(r);
for (int c = 0; c < m.cols; ++c)
{
const Vec3b& v = m(r,c);
*ptr = qRgb(v[2],v[1],v[0]);
++ptr;
}
}
return qim;
}
void GuiController :: saveCurrentFrames()
{
m_frame_recorder->saveCurrentFrames(lastImages());
}
void GuiController :: onRGBDDataUpdated()
{
if (m_paused && !m_process_one_frame)
return;
m_process_one_frame = false;
++m_frame_counter;
if (m_frame_counter == 10)
{
double current_tick = cv::getTickCount();
m_frame_rate = m_frame_counter / ((current_tick - m_last_tick)/cv::getTickFrequency());
m_last_tick = current_tick;
m_frame_counter = 0;
}
m_grabber.copyImagesTo(m_last_images);
if (m_active_device >= m_last_images.size())
{
ntk_dbg(0) << "Warning: no device " << m_active_device;
return;
}
TimeCount tc_rgbd_process("m_processor.processImage", 2);
for(int i = 0; i < m_last_images.size(); ++i)
{
m_processor.processImage(m_last_images[i]);
}
tc_rgbd_process.stop();
m_last_images[m_active_device].copyTo(m_last_image);
if (m_raw_images_window->isVisible())
m_raw_images_window->update(m_last_image);
if (m_frame_recorder && (m_screen_capture_mode || m_grab_frames))
{
saveCurrentFrames();
}
if (m_screen_capture_mode)
m_raw_window_grabber.saveFrame(QPixmap::grabWindow(m_raw_images_window->winId()));
if (m_mesh_generator && m_view3d_window->isVisible())
{
foreach_idx(i, m_last_images)
{
if (!m_merge_views && i != m_active_device)
continue;
if (!m_last_images[i].calibration())
{
ntk_dbg(0) << "Warning: image " << i << " does not have calibration.";
continue;
}
TimeCount tc_generate ("Mesh generate", 2);
m_mesh_generator->generate(m_last_images[i]);
tc_generate.stop();
TimeCount tc_add_mesh ("Add mesh", 2);
m_view3d_window->ui->mesh_view->addMesh(m_mesh_generator->mesh(), Pose3D(), MeshViewer::FLAT);
tc_add_mesh.stop();
}
if (m_screen_capture_mode)
m_view3d_window_grabber.saveFrame(QPixmap::grabWindow(m_view3d_window->winId()));
m_view3d_window->ui->mesh_view->swapScene();
}
QString status = QString("Final fps = %1 fps GRABBER = %2")
.arg(m_frame_rate, 0, 'f', 1)
.arg(m_grabber.frameRate(), 0, 'f', 1);
if (m_grab_frames)
status += " [GRABBING]";
m_raw_images_window->ui->statusbar->showMessage(status);
if (!m_paused)
m_grabber.newEvent();
}
void GuiController :: on_depth_mouse_moved(int x, int y)
{
if (!m_last_image.depth().data || x < 0 || y < 0)
return;
QString s = QString("Distance at (%1,%2) = %3 m")
.arg(x).arg(y).arg(m_last_image.depth()(y,x), 0, 'f', 3);
m_raw_images_window->ui->distanceLabel->setText(s);
}
void GuiController::toggleView3d(bool active)
{
if (active)
{
m_view3d_window->show();
}
else
{
m_view3d_window->hide();
}
}
void GuiController::toggleFilters(bool active)
{
if (active)
{
m_filters_window->show();
}
else
{
m_filters_window->hide();
}
}
void GuiController::setActiveDevice(int id)
{
m_active_device = id;
if (id < m_last_images.size() && m_view3d_window->isVisible() && m_last_images[id].calibration())
m_view3d_window->updateFromCalibration(m_last_images[id].calibration()->depth_pose->cvTranslation(),
m_last_images[id].calibration()->depth_pose->cvEulerRotation());
}
void GuiController::updateCameraCalibration(const cv::Vec3f &new_t, const cv::Vec3f &new_r)
{
if (m_active_device >= m_last_images.size())
return;
if (!m_last_images[m_active_device].calibration())
return;
// FIXME: some mutex are required here since these values might be read out from another grabber thread.
// FIXME: this is very risky and hacky.
RGBDCalibration* calibration = const_cast<RGBDCalibration*>(m_last_images[m_active_device].calibration());
Pose3D p;
p.applyTransformBefore(new_t, new_r);
p.cvRotationMatrixTranslation(calibration->T_extrinsics, calibration->R_extrinsics);
calibration->updatePoses();
}
void GuiController::refineCalibrationWithICP()
{
if (m_last_images.size() < 2)
{
ntk_dbg(1) << "Only one device, no need for alignment.";
return;
}
if (m_active_device == 0)
{
ntk_dbg(1) << "Active device is the same as the reference frame (0)";
return;
}
if (m_last_images.size() <= m_active_device)
{
ntk_dbg(1) << "No available image for active device";
return;
}
RelativePoseEstimatorICP estimator;
estimator.setReferenceImage(m_last_images[0]);
bool ok = estimator.estimateNewPose(m_last_images[m_active_device]);
if (!ok)
return;
*m_last_images[m_active_device].calibration()->depth_pose = estimator.currentPose();
cv::Vec3f t = estimator.currentPose().cvTranslation();
cv::Vec3f r = estimator.currentPose().cvEulerRotation();
updateCameraCalibration(t, r);
m_view3d_window->updateFromCalibration(t, r);
}
Jump to Line
Something went wrong with that request. Please try again.