/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPoint2DPDFGaussian::drawSingleSample(CPoint2D &outSample) const
{
MRPT_START
// Eigen3 emits an out-of-array warning here, but it seems to be a false warning? (WTF)
CVectorDouble vec;
randomGenerator.drawGaussianMultivariate(vec,cov);
ASSERT_(vec.size()==2);
outSample.x( mean.x() + vec[0] );
outSample.y( mean.y() + vec[1] );
MRPT_END
}
TEST(Matrices,fromMatlabStringFormat)
{
const char* mat1 = "[1 2 3;-3 -6 -5]";
const double vals1[] = {1,2,3,-3,-6,-5};
const char* mat2 = " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and spaces...
const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000 ,7};
const char* mat3 = "[9]";
const char* mat4 = "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
const char* mat5 = "[ ]"; // Empty
const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
const char* mat13 = "[9 8 7]";
const char* mat31 = "[9; 8; 7]";
CMatrixDouble M1,M2,M3, M4, M5, M6;
if (! M1.fromMatlabStringFormat(mat1) ||
(CMatrixFixedNumeric<double,2,3>(vals1)-M1).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat1;
{
CMatrixFixedNumeric<double,2,3> M1b;
if (! M1b.fromMatlabStringFormat(mat1) ||
(CMatrixFixedNumeric<double,2,3>(vals1)-M1b).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat1;
}
if (! M2.fromMatlabStringFormat(mat2) ||
M2.cols()!=2 || M2.rows()!=3 ||
(CMatrixFixedNumeric<double,3,2>(vals2)-M2).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat2;
{
CMatrixFixedNumeric<double,3,2> M2b;
if (! M2b.fromMatlabStringFormat(mat2) ||
(CMatrixFixedNumeric<double,3,2>(vals2)-M2b).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat2;
}
if (! M3.fromMatlabStringFormat(mat3) )
GTEST_FAIL() << mat3;
{
CVectorDouble m;
if (! m.fromMatlabStringFormat(mat3) || m.size()!=1 ) GTEST_FAIL() << "CVectorDouble:" << mat3;
}
{
CArrayDouble<1> m;
if (! m.fromMatlabStringFormat(mat3) ) GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
}
{
CVectorDouble m;
if (! m.fromMatlabStringFormat(mat31) || m.size()!=3 ) GTEST_FAIL() << "CVectorDouble:" << mat31;
}
{
CArrayDouble<3> m;
if (! m.fromMatlabStringFormat(mat31) ) GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
}
{
Eigen::Matrix<double,1,3> m;
if (! m.fromMatlabStringFormat(mat13) ) GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
}
{
Eigen::Matrix<double,1,Eigen::Dynamic> m;
if (! m.fromMatlabStringFormat(mat13) || m.size()!=3 ) GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
}
// This one MUST BE detected as WRONG:
if ( M4.fromMatlabStringFormat(mat4, NULL /*dont dump errors to cerr*/) )
GTEST_FAIL() << mat4;
if (! M5.fromMatlabStringFormat(mat5) || size(M5,1)!=0 || size(M5,2)!=0 )
GTEST_FAIL() << mat5;
if (! M6.fromMatlabStringFormat(mat6) )
GTEST_FAIL() << mat6;
// Check correct values loaded:
CMatrixDouble RES = M1*M2;
EXPECT_NEAR(0,(M6 - M1*M2).array().square().sum(), 1e-3);
}
// ------------------------------------------------------
// 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;
}