Exemple #1
0
// Add objects at your will to check results
void generateObjects(CSetOfObjects::Ptr& world)
{
	// create object, give it a random pose/color, insert it in the world
	CDisk::Ptr dsk = mrpt::make_aligned_shared<CDisk>();
	dsk->setDiskRadius(MYRANDG(5, 5), MYRANDG(5));
	configRandom(dsk);
	world->insert(dsk);

	CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(MYRANDG(5, 1));
	configRandom(sph);
	world->insert(sph);

	CTexturedPlane::Ptr pln = mrpt::make_aligned_shared<CTexturedPlane>(
		MYRANDG(10, -10), MYRANDG(10), MYRANDG(10, -10), MYRANDG(10));
	configRandom(pln);
	world->insert(pln);

	for (size_t i = 0; i < 5; i++)
	{
		CPolyhedron::Ptr poly =
			CPolyhedron::CreateRandomPolyhedron(MYRANDG(2, 2));
		configRandom(poly);
		world->insert(poly);
	}

	CCylinder::Ptr cil = mrpt::make_aligned_shared<CCylinder>(
		MYRANDG(3.0, 3.0), MYRANDG(3.0, 1.0), MYRANDG(2.0f, 3.0f), 50, 1);
	configRandom(cil);
	world->insert(cil);

	CEllipsoid::Ptr ell = mrpt::make_aligned_shared<CEllipsoid>();
	CMatrixDouble md = CMatrixDouble(3, 3);
	for (size_t i = 0; i < 3; i++) md(i, i) = MYRANDG(8.0, 1.0);
	for (size_t i = 0; i < 3; i++)
	{
		size_t ii = (i + 1) % 3;
		md(i, ii) = md(ii, i) = MYRANDG(sqrt(md(i, i) * md(ii, ii)));
	}
	ell->setCovMatrix(md);
	configRandom(std::dynamic_pointer_cast<CRenderizable>(ell));
	world->insert(ell);
}
void CDifodoDatasets::updateScene()
{
	CPose3D rel_lenspose(0, -0.022, 0, 0, 0, 0);

	scene = window.get3DSceneAndLock();

	// Reference gt
	CSetOfObjects::Ptr reference_gt = scene->getByClass<CSetOfObjects>(0);
	reference_gt->setPose(gt_pose);

	// Camera points
	CPointCloudColoured::Ptr cam_points =
		scene->getByClass<CPointCloudColoured>(0);
	cam_points->clear();
	cam_points->setPose(gt_pose);
	for (unsigned int y = 0; y < cols; y++)
		for (unsigned int z = 0; z < rows; z++)
			cam_points->push_back(
				depth[repr_level](z, y), xx[repr_level](z, y),
				yy[repr_level](z, y), 1.f - sqrt(weights(z, y)),
				sqrt(weights(z, y)), 0);

	// DifOdo camera
	CBox::Ptr camera_odo = scene->getByClass<CBox>(0);
	camera_odo->setPose(cam_pose + rel_lenspose);

	// Groundtruth camera
	CBox::Ptr camera_gt = scene->getByClass<CBox>(1);
	camera_gt->setPose(gt_pose + rel_lenspose);

	// Frustum
	CFrustum::Ptr FOV = scene->getByClass<CFrustum>(0);
	FOV->setPose(gt_pose);

	if ((first_pose == true) && ((gt_pose - gt_oldpose).norm() < 0.5))
	{
		// Difodo traj lines
		CSetOfLines::Ptr traj_lines_odo = scene->getByClass<CSetOfLines>(0);
		traj_lines_odo->appendLine(
			cam_oldpose.x(), cam_oldpose.y(), cam_oldpose.z(), cam_pose.x(),
			cam_pose.y(), cam_pose.z());

		// Difodo traj points
		CPointCloud::Ptr traj_points_odo = scene->getByClass<CPointCloud>(0);
		traj_points_odo->insertPoint(cam_pose.x(), cam_pose.y(), cam_pose.z());

		// Groundtruth traj lines
		CSetOfLines::Ptr traj_lines_gt = scene->getByClass<CSetOfLines>(1);
		traj_lines_gt->appendLine(
			gt_oldpose.x(), gt_oldpose.y(), gt_oldpose.z(), gt_pose.x(),
			gt_pose.y(), gt_pose.z());

		// Groundtruth traj points
		CPointCloud::Ptr traj_points_gt = scene->getByClass<CPointCloud>(1);
		traj_points_gt->insertPoint(gt_pose.x(), gt_pose.y(), gt_pose.z());
	}

	// Ellipsoid showing covariance
	math::CMatrixFloat33 cov3d = 20.f * est_cov.topLeftCorner(3, 3);
	CEllipsoid::Ptr ellip = scene->getByClass<CEllipsoid>(0);
	ellip->setCovMatrix(cov3d);
	ellip->setPose(cam_pose + rel_lenspose);

	window.unlockAccess3DScene();
	window.repaint();
}
void CDifodoDatasets::initializeScene()
{
	CPose3D rel_lenspose(0, -0.022, 0, 0, 0, 0);

	global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE(1000000);
	window.resize(1000, 900);
	window.setPos(900, 0);
	window.setCameraZoom(16);
	window.setCameraAzimuthDeg(0);
	window.setCameraElevationDeg(90);
	window.setCameraPointingToPoint(0, 0, 0);
	window.setCameraPointingToPoint(0, 0, 1);

	scene = window.get3DSceneAndLock();

	// Lights:
	scene->getViewport()->setNumberOfLights(1);
	CLight& light0 = scene->getViewport()->getLight(0);
	light0.light_ID = 0;
	light0.setPosition(0, 0, 1, 1);

	// Grid (ground)
	CGridPlaneXY::Ptr ground = mrpt::make_aligned_shared<CGridPlaneXY>();
	scene->insert(ground);

	// Reference
	// CSetOfObjects::Ptr reference = stock_objects::CornerXYZ();
	// scene->insert( reference );

	//					Cameras and points
	//------------------------------------------------------

	// DifOdo camera
	CBox::Ptr camera_odo = mrpt::make_aligned_shared<CBox>(
		math::TPoint3D(-0.02, -0.1, -0.01), math::TPoint3D(0.02, 0.1, 0.01));
	camera_odo->setPose(cam_pose + rel_lenspose);
	camera_odo->setColor(0, 1, 0);
	scene->insert(camera_odo);

	// Groundtruth camera
	CBox::Ptr camera_gt = mrpt::make_aligned_shared<CBox>(
		math::TPoint3D(-0.02, -0.1, -0.01), math::TPoint3D(0.02, 0.1, 0.01));
	camera_gt->setPose(gt_pose + rel_lenspose);
	camera_gt->setColor(1, 0, 0);
	scene->insert(camera_gt);

	// Frustum
	opengl::CFrustum::Ptr FOV = mrpt::make_aligned_shared<opengl::CFrustum>(
		0.3f, 2, 57.3f * fovh, 57.3f * fovv, 1.f, true, false);
	FOV->setColor(0.7, 0.7, 0.7);
	FOV->setPose(gt_pose);
	scene->insert(FOV);

	// Reference gt
	CSetOfObjects::Ptr reference_gt = stock_objects::CornerXYZ();
	reference_gt->setScale(0.2f);
	reference_gt->setPose(gt_pose);
	scene->insert(reference_gt);

	// Camera points
	CPointCloudColoured::Ptr cam_points =
		mrpt::make_aligned_shared<CPointCloudColoured>();
	cam_points->setColor(1, 0, 0);
	cam_points->setPointSize(2);
	cam_points->enablePointSmooth(1);
	cam_points->setPose(cam_pose);
	scene->insert(cam_points);

	//					Trajectories and covariance
	//-------------------------------------------------------------

	// Dif Odometry
	CSetOfLines::Ptr traj_lines_odo = mrpt::make_aligned_shared<CSetOfLines>();
	traj_lines_odo->setLocation(0, 0, 0);
	traj_lines_odo->setColor(0, 0.6, 0);
	traj_lines_odo->setLineWidth(6);
	scene->insert(traj_lines_odo);
	CPointCloud::Ptr traj_points_odo = mrpt::make_aligned_shared<CPointCloud>();
	traj_points_odo->setColor(0, 0.6, 0);
	traj_points_odo->setPointSize(4);
	traj_points_odo->enablePointSmooth(1);
	scene->insert(traj_points_odo);

	// Groundtruth
	CSetOfLines::Ptr traj_lines_gt = mrpt::make_aligned_shared<CSetOfLines>();
	traj_lines_gt->setLocation(0, 0, 0);
	traj_lines_gt->setColor(0.6, 0, 0);
	traj_lines_gt->setLineWidth(6);
	scene->insert(traj_lines_gt);
	CPointCloud::Ptr traj_points_gt = mrpt::make_aligned_shared<CPointCloud>();
	traj_points_gt->setColor(0.6, 0, 0);
	traj_points_gt->setPointSize(4);
	traj_points_gt->enablePointSmooth(1);
	scene->insert(traj_points_gt);

	// Ellipsoid showing covariance
	math::CMatrixFloat33 cov3d = 20.f * est_cov.topLeftCorner(3, 3);
	CEllipsoid::Ptr ellip = mrpt::make_aligned_shared<CEllipsoid>();
	ellip->setCovMatrix(cov3d);
	ellip->setQuantiles(2.0);
	ellip->setColor(1.0, 1.0, 1.0, 0.5);
	ellip->enableDrawSolid3D(true);
	ellip->setPose(cam_pose + rel_lenspose);
	scene->insert(ellip);

	// User-interface information
	utils::CImage img_legend;
	img_legend.loadFromXPM(legend_xpm);
	COpenGLViewport::Ptr legend = scene->createViewport("legend");
	legend->setViewportPosition(20, 20, 332, 164);
	legend->setImageView(img_legend);

	window.unlockAccess3DScene();
	window.repaint();
}