// ------------------------------------------------------
// TestCameraCaptureAsk
// ------------------------------------------------------
void TestCameraCaptureAsk()
{
CCameraSensor::Ptr cam = prepareVideoSourceFromUserSelection();
if (!cam)
{
cerr << "The user didn't pick any camera. Exiting." << endl;
return;
}
CDisplayWindow win("Live video");
cout << "Close the window to exit." << endl;
double counter = 0;
mrpt::system::CTicTac tictac;
while (win.isOpen())
{
if( !counter )
tictac.Tic();
mrpt::obs::CObservation::Ptr obs = cam->getNextFrame();
ASSERT_(obs);
if (IS_CLASS(obs,CObservationImage))
{
CObservationImage::Ptr o=CObservationImage::Ptr(obs);
win.showImage(o->image);
}
else if (IS_CLASS(obs,CObservationStereoImages))
{
CObservationStereoImages::Ptr o=CObservationStereoImages::Ptr(obs);
win.showImage(o->imageRight);
}
if( ++counter == 10 )
{
double t = tictac.Tac();
cout << "Frame Rate: " << counter/t << " fps" << endl;
counter = 0;
}
std::this_thread::sleep_for(2ms);
}
cout << "Closing..." << endl;
}
// ------------------------------------------------------
// TestCamera3DFaceDetection
// ------------------------------------------------------
void TestCamera3DFaceDetection(CCameraSensor::Ptr cam)
{
CDisplayWindow win("Live video");
CDisplayWindow win2("FaceDetected");
cout << "Close the window to exit." << endl;
mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);
mrpt::gui::CDisplayWindow3D win3D2;
win3D.setCameraAzimuthDeg(140);
win3D.setCameraElevationDeg(20);
win3D.setCameraZoom(6.0);
win3D.setCameraPointingToPoint(2.5, 0, 0);
mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
mrpt::opengl::COpenGLScene::Ptr scene2;
mrpt::opengl::CPointCloudColoured::Ptr gl_points =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points->setPointSize(4.5);
mrpt::opengl::CPointCloudColoured::Ptr gl_points2 =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points2->setPointSize(4.5);
// Create the Opengl object for the point cloud:
scene->insert(gl_points);
scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
scene->insert(mrpt::opengl::stock_objects::CornerXYZ());
win3D.unlockAccess3DScene();
if (showEachDetectedFace)
{
win3D2.setWindowTitle("3D Face detected");
win3D2.resize(400, 300);
win3D2.setCameraAzimuthDeg(140);
win3D2.setCameraElevationDeg(20);
win3D2.setCameraZoom(6.0);
win3D2.setCameraPointingToPoint(2.5, 0, 0);
scene2 = win3D2.get3DSceneAndLock();
scene2->insert(gl_points2);
scene2->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
win3D2.unlockAccess3DScene();
}
double counter = 0;
mrpt::utils::CTicTac tictac;
CVectorDouble fps;
while (win.isOpen())
{
if (!counter) tictac.Tic();
CObservation3DRangeScan::Ptr o;
try
{
o = std::dynamic_pointer_cast<CObservation3DRangeScan>(
cam->getNextFrame());
}
catch (CExceptionEOF&)
{
break;
}
ASSERT_(o);
vector_detectable_object detected;
// CObservation3DRangeScan::Ptr o =
// std::dynamic_pointer_cast<CObservation3DRangeScan>(obs);
faceDetector.detectObjects(o, detected);
// static int x = 0;
if (detected.size() > 0)
{
for (unsigned int i = 0; i < detected.size(); i++)
{
ASSERT_(IS_CLASS(detected[i], CDetectable3D))
CDetectable3D::Ptr obj =
std::dynamic_pointer_cast<CDetectable3D>(detected[i]);
if (showEachDetectedFace)
{
CObservation3DRangeScan face;
o->getZoneAsObs(
face, obj->m_y, obj->m_y + obj->m_height, obj->m_x,
obj->m_x + obj->m_width);
win2.showImage(face.intensityImage);
if (o->hasPoints3D)
{
win3D2.get3DSceneAndLock();
CColouredPointsMap pntsMap;
if (!o->hasConfidenceImage)
{
pntsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage;
pntsMap.loadFromRangeScan(face);
}
else
{
vector<float> xs, ys, zs;
unsigned int i = 0;
for (unsigned int j = 0;
j < face.confidenceImage.getHeight(); j++)
for (unsigned int k = 0;
k < face.confidenceImage.getWidth();
k++, i++)
{
unsigned char c =
*(face.confidenceImage.get_unsafe(
k, j, 0));
if (c > faceDetector.m_options
.confidenceThreshold)
{
xs.push_back(face.points3D_x[i]);
ys.push_back(face.points3D_y[i]);
zs.push_back(face.points3D_z[i]);
}
}
pntsMap.setAllPoints(xs, ys, zs);
}
gl_points2->loadFromPointsMap(&pntsMap);
win3D2.unlockAccess3DScene();
win3D2.repaint();
}
}
o->intensityImage.rectangle(
obj->m_x, obj->m_y, obj->m_x + obj->m_width,
obj->m_y + obj->m_height, TColor(255, 0, 0));
// x++;
// if (( showEachDetectedFace ) && ( x > 430 ) )
// system::pause();
}
}
win.showImage(o->intensityImage);
/*if (( showEachDetectedFace ) && ( detected.size() ))
system::pause();*/
win3D.get3DSceneAndLock();
CColouredPointsMap pntsMap;
pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
pntsMap.loadFromRangeScan(*(o.get()));
gl_points->loadFromPointsMap(&pntsMap);
win3D.unlockAccess3DScene();
win3D.repaint();
if (++counter == 10)
{
double t = tictac.Tac();
cout << "Frame Rate: " << counter / t << " fps" << endl;
fps.push_back(counter / t);
counter = 0;
}
std::this_thread::sleep_for(2ms);
}
cout << "Fps mean: " << fps.sumAll() / fps.size() << endl;
faceDetector.experimental_showMeasurements();
cout << "Closing..." << endl;
}
// ------------------------------------------------------
// TestCameraCaptureAsk
// ------------------------------------------------------
void TestCameraCaptureAsk()
{
CCameraSensor::Ptr cam = prepareVideoSourceFromUserSelection();
if (!cam)
{
cerr << "The user didn't pick any camera. Exiting." << endl;
return;
}
CDisplayWindow win("Live video");
cout << "Press 's' to save frames.\nClose the window to exit.\n";
double counter = 0;
mrpt::utils::CTicTac tictac;
while (win.isOpen())
{
if (!counter) tictac.Tic();
mrpt::obs::CObservation::Ptr obs = cam->getNextFrame();
ASSERT_(obs);
CImage* img = nullptr;
if (IS_CLASS(obs, CObservationImage))
{
CObservationImage::Ptr o =
std::dynamic_pointer_cast<CObservationImage>(obs);
img = &o->image;
}
else if (IS_CLASS(obs, CObservationStereoImages))
{
CObservationStereoImages::Ptr o =
std::dynamic_pointer_cast<CObservationStereoImages>(obs);
img = &o->imageRight;
}
else if (IS_CLASS(obs, CObservation3DRangeScan))
{
CObservation3DRangeScan::Ptr o =
std::dynamic_pointer_cast<CObservation3DRangeScan>(obs);
if (o->hasIntensityImage) img = &o->intensityImage;
}
if (img) win.showImage(*img);
if (++counter == 10)
{
double t = tictac.Tac();
cout << "Frame Rate: " << counter / t << " fps" << endl;
counter = 0;
}
// Process keystrokes:
if (mrpt::system::os::kbhit())
{
const int key_code = mrpt::system::os::getch();
switch (key_code)
{
case 's':
case 'S':
{
static int cnt = 0;
const std::string sFile =
mrpt::format("frame%05i.png", cnt++);
cout << "Saving frame to: " << sFile << endl;
img->saveToFile(sFile);
}
break;
default:
break;
};
}
std::this_thread::sleep_for(2ms);
}
cout << "Closing..." << endl;
}
// ------------------------------------------------------
// TestCameraFaceDetection
// ------------------------------------------------------
void TestCameraFaceDetection()
{
CCameraSensor::Ptr cam = prepareVideoSourceFromUserSelection();
if (!cam)
{
cerr << "The user didn't pick any camera. Exiting." << endl;
return;
}
mrpt::obs::CObservation::Ptr obs = cam->getNextFrame();
ASSERT_(obs);
if (IS_CLASS(obs, CObservation3DRangeScan))
{
TestCamera3DFaceDetection(cam);
return;
}
CDisplayWindow win("Live video");
cout << "Close the window to exit." << endl;
double counter = 0;
mrpt::utils::CTicTac tictac;
while (win.isOpen())
{
if (!counter) tictac.Tic();
mrpt::obs::CObservation::Ptr obs;
try
{
obs = cam->getNextFrame();
}
catch (CExceptionEOF&) // Check if eof, f.i. for RawLog files
{
break;
}
ASSERT_(obs);
if (IS_CLASS(obs, CObservationImage))
{
vector_detectable_object detected;
faceDetector.detectObjects(obs, detected);
CObservationImage::Ptr o =
std::dynamic_pointer_cast<CObservationImage>(obs);
for (unsigned int i = 0; i < detected.size(); i++)
{
ASSERT_(IS_CLASS(detected[i], CDetectable2D))
CDetectable2D::Ptr obj =
std::dynamic_pointer_cast<CDetectable2D>(detected[i]);
o->image.rectangle(
obj->m_x, obj->m_y, obj->m_x + obj->m_width,
obj->m_y + obj->m_height, TColor(255, 0, 0));
}
win.showImage(o->image);
}
else if (IS_CLASS(obs, CObservationStereoImages))
{
vector_detectable_object detected;
faceDetector.detectObjects(obs, detected);
CObservationStereoImages::Ptr o =
std::dynamic_pointer_cast<CObservationStereoImages>(obs);
for (unsigned int i = 0; i < detected.size(); i++)
{
ASSERT_(IS_CLASS(detected[i], CDetectable2D))
CDetectable2D::Ptr obj =
std::dynamic_pointer_cast<CDetectable2D>(detected[i]);
o->imageRight.rectangle(
obj->m_x, obj->m_y, obj->m_x + obj->m_width,
obj->m_y + obj->m_height, TColor(255, 0, 0));
}
win.showImage(o->imageRight);
}
if (++counter == 10)
{
double t = tictac.Tac();
cout << "Frame Rate: " << counter / t << " fps" << endl;
counter = 0;
}
std::this_thread::sleep_for(2ms);
}
cout << "Closing..." << endl;
}