Example #1
0
/*---------------------------------------------------------------
					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);
}
Example #3
0
// ------------------------------------------------------
//				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;
}