Esempio n. 1
0
CSetOfObjects::Ptr stock_objects::Hokuyo_UTM()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	{
		CBox::Ptr base = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-0.03, -0.03, -0.055), TPoint3D(0.03, 0.03, -0.014));
		base->setColor(0, 0, 0);
		ret->insert(base);
	}
	{
		CCylinder::Ptr cyl1 =
			mrpt::make_aligned_shared<CCylinder>(0.028f, 0.024f, 0.028f);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(0, 0, -0.014));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			mrpt::make_aligned_shared<CCylinder>(0.028f, 0.028f, 0.01f);
		cyl2->setColor(1, 69 / 255.0, 0);
		cyl2->setLocation(0, 0, 0.014);
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl3 =
			mrpt::make_aligned_shared<CCylinder>(0.028f, 0.028f, 0.01f);
		cyl3->setColor(0, 0, 0);
		cyl3->setLocation(0, 0, 0.024);
		ret->insert(cyl3);
	}

	return ret;
}
Esempio n. 2
0
CSetOfObjects::Ptr stock_objects::CornerXYZSimple(float scale, float lineWidth)
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	{
		CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
		lin->setLineWidth(lineWidth);
		lin->setColor(1, 0, 0);
		lin->setLineCoords(0, 0, 0, scale, 0, 0);
		ret->insert(lin);
	}
	{
		CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
		lin->setLineWidth(lineWidth);
		lin->setColor(0, 1, 0);
		lin->setLineCoords(0, 0, 0, 0, scale, 0);
		ret->insert(lin);
	}
	{
		CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
		lin->setLineWidth(lineWidth);
		lin->setColor(0, 0, 1);
		lin->setLineCoords(0, 0, 0, 0, 0, scale);
		ret->insert(lin);
	}
	return ret;
}
Esempio n. 3
0
CSetOfObjects::Ptr stock_objects::Hokuyo_URG()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	{
		CBox::Ptr base = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-0.025, -0.025, -0.0575), TPoint3D(0.025, 0.025, -0.0185));
		base->setColor(0.7, 0.7, 0.7);
		ret->insert(base);
	}
	{
		CCylinder::Ptr cyl1 =
			mrpt::make_aligned_shared<CCylinder>(0.02f, 0.02f, 0.01f);
		cyl1->setColor(0, 0, 0);
		cyl1->setLocation(0, 0, -0.014);
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			mrpt::make_aligned_shared<CCylinder>(0.02f, 0.0175f, 0.01f);
		cyl2->setColor(0, 0, 0);
		cyl2->setLocation(0, 0, -0.004);
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl3 =
			mrpt::make_aligned_shared<CCylinder>(0.0175f, 0.0175f, 0.01f);
		cyl3->setColor(0, 0, 0);
		cyl3->setLocation(0, 0, 0.004);
		ret->insert(cyl3);
	}

	return ret;
}
Esempio n. 4
0
/*---------------------------------------------------------------
					CornerXYZ
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::CornerXYZ(float scale)
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	CArrow::Ptr obj = CArrow::Create(
		0, 0, 0, scale, 0, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);

	obj->setColor(1, 0, 0);

	ret->insert(obj);

	obj = CArrow::Create(
		0, 0, 0, 0, scale, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);
	obj->setColor(0, 1, 0);

	ret->insert(obj);

	obj = CArrow::Create(
		0, 0, 0, 0, 0, scale, 0.25f * scale, 0.02f * scale, 0.05f * scale);
	obj->setColor(0, 0, 1);

	ret->insert(obj);

	return ret;
}
Esempio n. 5
0
/*---------------------------------------------------------------
					BumblebeeCamera
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::BumblebeeCamera()
{
	CSetOfObjects::Ptr camera =
		mrpt::make_aligned_shared<opengl::CSetOfObjects>();

	CPolyhedron::Ptr rect = opengl::CPolyhedron::CreateCubicPrism(
		-0.02, 0.14, -0.02, 0.02, 0, -0.04);
	rect->setColor(1, 0.8, 0);

	camera->insert(rect);

	CCylinder::Ptr lCam = mrpt::make_aligned_shared<opengl::CCylinder>(
		0.01f, 0.01f, 0.003f, 10, 10);
	lCam->setColor(1, 0, 0);

	CCylinder::Ptr rCam = mrpt::make_aligned_shared<opengl::CCylinder>(
		0.01f, 0.01f, 0.003f, 10, 10);
	rCam->setPose(CPose3D(0.12, 0, 0));
	rCam->setColor(0, 0, 0);

	camera->insert(lCam);
	camera->insert(rCam);

	return camera;
}
Esempio n. 6
0
CRobotPose::Ptr CGlWidget::removePoseFromPointsCloud(
	CSetOfObjects::Ptr points, int index) const
{
	auto it = points->begin() + index;
	ASSERT_(it != points->end());
	CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(*it);
	ASSERT_(robotPose);
	points->removeObject(*it);
	return robotPose;
}
Esempio n. 7
0
mrpt::opengl::CSetOfObjects::Ptr framePosesVecVisualize(
	const TFramePosesVec& poses, const double len, const double lineWidth)
{
	mrpt::opengl::CSetOfObjects::Ptr obj =
		mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();

	for (size_t i = 0; i < poses.size(); i++)
	{
		CSetOfObjects::Ptr corner =
			opengl::stock_objects::CornerXYZSimple(len, lineWidth);
		CPose3D p = poses[i];
		p.x(WORLD_SCALE * p.x());
		p.y(WORLD_SCALE * p.y());
		p.z(WORLD_SCALE * p.z());
		corner->setPose(p);
		corner->setName(format("%u", (unsigned int)i));
		corner->enableShowName();
		obj->insert(corner);
	}
	return obj;
}
Esempio n. 8
0
/**
 * Generate 3 objects to work with - 1 sphere, 2 disks
 */
void generateObjects(CSetOfObjects::Ptr& world)
{
	CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.5);
	sph->setLocation(0, 0, 0);
	sph->setColor(1, 0, 0);
	world->insert(sph);

	CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
	pln->setDiskRadius(2);
	pln->setPose(CPose3D(0, 0, 0, 0, DEG2RAD(5), DEG2RAD(5)));
	pln->setColor(0.8, 0, 0);
	world->insert(pln);

	{
		CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
		pln->setDiskRadius(2);
		pln->setPose(CPose3D(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(-2)));
		pln->setColor(0.9, 0, 0);
		world->insert(pln);
	}
}
Esempio n. 9
0
/*---------------------------------------------------------------
					RobotRhodon
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotRhodon()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
	float height = 0;

	vector<TPoint2D> level1;
	level1.push_back(TPoint2D(0.31, 0));
	level1.push_back(TPoint2D(0.22, 0.24));
	level1.push_back(TPoint2D(-0.22, 0.24));
	level1.push_back(TPoint2D(-0.31, 0));
	level1.push_back(TPoint2D(-0.22, -0.24));
	level1.push_back(TPoint2D(0.22, -0.24));

	CPolyhedron::Ptr obj1 =
		opengl::CPolyhedron::CreateCustomPrism(level1, 0.38);
	obj1->setLocation(0, 0, height);
	height += 0.38f;
	obj1->setColor(0.6, 0.6, 0.6);
	ret->insert(obj1);

	vector<TPoint2D> level2;
	level2.push_back(TPoint2D(0.16, 0.21));
	level2.push_back(TPoint2D(-0.16, 0.21));
	level2.push_back(TPoint2D(-0.16, -0.21));
	level2.push_back(TPoint2D(0.16, -0.21));

	CPolyhedron::Ptr obj2 =
		opengl::CPolyhedron::CreateCustomPrism(level2, 0.35);
	obj2->setLocation(0, 0, height);
	height += 0.35f;
	obj2->setColor(0.2, 0.2, 0.2);
	ret->insert(obj2);

	vector<TPoint2D> level3;
	level3.push_back(TPoint2D(-0.12, 0.12));
	level3.push_back(TPoint2D(-0.16, 0.12));
	level3.push_back(TPoint2D(-0.16, -0.12));
	level3.push_back(TPoint2D(-0.12, -0.12));

	CPolyhedron::Ptr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 1);
	obj3->setLocation(0, 0, height);
	// height+=1;
	obj3->setColor(0.6, 0.6, 0.6);
	ret->insert(obj3);

	opengl::CCylinder::Ptr obj4 = mrpt::make_aligned_shared<opengl::CCylinder>(
		0.05f, 0.05f, 0.4f, 20, 20);
	obj4->setLocation(0, 0, 0.73);
	obj4->setColor(0, 0, 0.9);
	ret->insert(obj4);

	opengl::CCylinder::Ptr obj5 = mrpt::make_aligned_shared<opengl::CCylinder>(
		0.05f, 0.05f, 0.4f, 20, 20);
	obj5->setPose(CPose3D(0.32, 0, 0.89, 0, -1, 0));
	obj5->setColor(0, 0, 0.9);
	ret->insert(obj5);

	return ret;
}
Esempio n. 10
0
CSetOfObjects::Ptr stock_objects::CornerXYZEye()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
	CPose3D rotation;

	CArrow::Ptr obj = CArrow::Create(0, 0, 0, 1.0, 0, 0, 0.25f, 0.02f, 0.05f);

	obj->setColor(1, 0, 0);

	ret->insert(obj);

	obj = CArrow::Create(0, 0, 0, 0, 1.0, 0, 0.25f, 0.02f, 0.05f);
	obj->setColor(0, 1, 0);

	ret->insert(obj);

	obj = CArrow::Create(0, 0, -1.0, 0, 0, 0, 0.25f, 0.02f, 0.05f);
	obj->setColor(0, 0, 1);

	ret->insert(obj);

	return ret;
}
Esempio n. 11
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);
}
Esempio n. 12
0
// ------------------------------------------------------
//				Test_Kinect
// ------------------------------------------------------
void Test_Kinect()
{
	// Launch grabbing thread:
	// --------------------------------------------------------
	TThreadParam thrPar;
	std::thread thHandle = std::thread(thread_grabbing, std::ref(thrPar));

	// Wait until data stream starts so we can say for sure the sensor has been
	// initialized OK:
	cout << "Waiting for sensor initialization...\n";
	do
	{
		CObservation3DRangeScan::Ptr possiblyNewObs =
			std::atomic_load(&thrPar.new_obs);
		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
			break;
		else
			std::this_thread::sleep_for(10ms);
	} while (!thrPar.quit);

	// Check error condition:
	if (thrPar.quit) return;

	// Feature tracking variables:
	CFeatureList trackedFeats;
	unsigned int step_num = 0;

	bool SHOW_FEAT_IDS = true;
	bool SHOW_RESPONSES = true;

	CGenericFeatureTrackerAutoPtr tracker;

	// "CFeatureTracker_KL" is by far the most robust implementation for now:
	tracker = CGenericFeatureTrackerAutoPtr(new CFeatureTracker_KL);
	tracker->enableTimeLogger(true);  // Do time profiling.

	// Set of parameters common to any tracker implementation:
	// To see all the existing params and documentation, see
	// mrpt::vision::CGenericFeatureTracker
	//  http://reference.mrpt.org/devel/structmrpt_1_1vision_1_1_c_generic_feature_tracker.html
	tracker->extra_params["add_new_features"] =
		1;  // track, AND ALSO, add new features
	tracker->extra_params["add_new_feat_min_separation"] = 25;
	tracker->extra_params["add_new_feat_max_features"] = 150;
	tracker->extra_params["add_new_feat_patch_size"] = 21;

	tracker->extra_params["minimum_KLT_response_to_add"] = 40;
	tracker->extra_params["check_KLT_response_every"] =
		5;  // Re-check the KLT-response to assure features are in good points.
	tracker->extra_params["minimum_KLT_response"] =
		25;  // Re-check the KLT-response to assure features are in good points.

	tracker->extra_params["update_patches_every"] = 0;  // Update patches

	// Specific params for "CFeatureTracker_KL"
	tracker->extra_params["window_width"] = 25;
	tracker->extra_params["window_height"] = 25;

	// Global points map:
	CColouredPointsMap globalPtsMap;
	globalPtsMap.colorScheme.scheme =
		CColouredPointsMap::cmFromIntensityImage;  // Take points color from
	// RGB+D observations
	// globalPtsMap.colorScheme.scheme =
	// CColouredPointsMap::cmFromHeightRelativeToSensorGray;

	// Create window and prepare OpenGL object in the scene:
	// --------------------------------------------------------
	mrpt::gui::CDisplayWindow3D win3D("kinect-3d-slam 3D view", 800, 600);

	win3D.setCameraAzimuthDeg(140);
	win3D.setCameraElevationDeg(20);
	win3D.setCameraZoom(8.0);
	win3D.setFOV(90);
	win3D.setCameraPointingToPoint(2.5, 0, 0);

	mrpt::opengl::CPointCloudColoured::Ptr gl_points =
		mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
	gl_points->setPointSize(2.5);

	mrpt::opengl::CSetOfObjects::Ptr gl_curFeats =
		mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
	mrpt::opengl::CSetOfObjects::Ptr gl_keyframes =
		mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();

	mrpt::opengl::CPointCloudColoured::Ptr gl_points_map =
		mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
	gl_points_map->setPointSize(2.0);

	const double aspect_ratio =
		480.0 / 640.0;  // kinect.rows() / double( kinect.cols() );

	mrpt::opengl::CSetOfObjects::Ptr gl_cur_cam_corner =
		mrpt::opengl::stock_objects::CornerXYZSimple(0.4f, 4);

	opengl::COpenGLViewport::Ptr viewInt;
	{
		mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();

		// Create the Opengl object for the point cloud:
		scene->insert(gl_points_map);
		scene->insert(gl_points);
		scene->insert(gl_curFeats);
		scene->insert(gl_keyframes);
		scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());

		scene->insert(gl_cur_cam_corner);

		const int VW_WIDTH =
			350;  // Size of the viewport into the window, in pixel units.
		const int VW_HEIGHT = aspect_ratio * VW_WIDTH;

		// Create the Opengl objects for the planar images each in a separate
		// viewport:
		viewInt = scene->createViewport("view2d_int");
		viewInt->setViewportPosition(2, 2, VW_WIDTH, VW_HEIGHT);
		viewInt->setTransparent(true);

		win3D.unlockAccess3DScene();
		win3D.repaint();
	}

	CImage previous_image;

	map<TFeatureID, TPoint3D> lastVisibleFeats;
	std::vector<TPose3D>
		camera_key_frames_path;  // The 6D path of the Kinect camera.
	CPose3D
		currentCamPose_wrt_last;  // wrt last pose in "camera_key_frames_path"

	bool gl_keyframes_must_refresh =
		true;  // Need to update gl_keyframes from camera_key_frames_path??
	CObservation3DRangeScan::Ptr last_obs;
	string str_status, str_status2;

	while (win3D.isOpen() && !thrPar.quit)
	{
		CObservation3DRangeScan::Ptr possiblyNewObs =
			std::atomic_load(&thrPar.new_obs);
		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
			(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
		{
			// It IS a new observation:
			last_obs = possiblyNewObs;

			// Feature tracking -------------------------------------------
			ASSERT_(last_obs->hasIntensityImage);

			CImage theImg;  // The grabbed image:
			theImg = last_obs->intensityImage;

			// Do tracking:
			if (step_num > 1)  // we need "previous_image" to be valid.
			{
				tracker->trackFeatures(previous_image, theImg, trackedFeats);

				// Remove those now out of the image plane:
				CFeatureList::iterator itFeat = trackedFeats.begin();
				while (itFeat != trackedFeats.end())
				{
					const TFeatureTrackStatus status = (*itFeat)->track_status;
					bool eras =
						(status_TRACKED != status && status_IDLE != status);
					if (!eras)
					{
						// Also, check if it's too close to the image border:
						const float x = (*itFeat)->x;
						const float y = (*itFeat)->y;
						static const float MIN_DIST_MARGIN_TO_STOP_TRACKING =
							10;
						if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
							y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
							x > (last_obs->cameraParamsIntensity.ncols -
								 MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
							y > (last_obs->cameraParamsIntensity.nrows -
								 MIN_DIST_MARGIN_TO_STOP_TRACKING))
						{
							eras = true;
						}
					}
					if (eras)  // Erase or keep?
						itFeat = trackedFeats.erase(itFeat);
					else
						++itFeat;
				}
			}

			// Create list of 3D features in space, wrt current camera pose:
			// --------------------------------------------------------------------
			map<TFeatureID, TPoint3D> curVisibleFeats;
			for (CFeatureList::iterator itFeat = trackedFeats.begin();
				 itFeat != trackedFeats.end(); ++itFeat)
			{
				// Pixel coordinates in the intensity image:
				const int int_x = (*itFeat)->x;
				const int int_y = (*itFeat)->y;

				// Convert to pixel coords in the range image:
				//  APPROXIMATION: Assume coordinates are equal (that's not
				//  exact!!)
				const int x = int_x;
				const int y = int_y;

				// Does this (x,y) have valid range data?
				const float d = last_obs->rangeImage(y, x);
				if (d > 0.05 && d < 10.0)
				{
					ASSERT_(
						size_t(
							last_obs->rangeImage.cols() *
							last_obs->rangeImage.rows()) ==
						last_obs->points3D_x.size());
					const size_t nPt = last_obs->rangeImage.cols() * y + x;
					curVisibleFeats[(*itFeat)->ID] = TPoint3D(
						last_obs->points3D_x[nPt], last_obs->points3D_y[nPt],
						last_obs->points3D_z[nPt]);
				}
			}

			// Load local points map from 3D points + color:
			CColouredPointsMap localPntsMap;
			localPntsMap.colorScheme.scheme =
				CColouredPointsMap::cmFromIntensityImage;
			localPntsMap.loadFromRangeScan(*last_obs);

			// Estimate our current camera pose from feature2feature matching:
			// --------------------------------------------------------------------
			if (!lastVisibleFeats.empty())
			{
				TMatchingPairList corrs;  // pairs of correspondences

				for (map<TFeatureID, TPoint3D>::const_iterator itCur =
						 curVisibleFeats.begin();
					 itCur != curVisibleFeats.end(); ++itCur)
				{
					map<TFeatureID, TPoint3D>::const_iterator itFound =
						lastVisibleFeats.find(itCur->first);
					if (itFound != lastVisibleFeats.end())
					{
						corrs.push_back(
							TMatchingPair(
								itFound->first, itCur->first, itFound->second.x,
								itFound->second.y, itFound->second.z,
								itCur->second.x, itCur->second.y,
								itCur->second.z));
					}
				}

				if (corrs.size() >= 3)
				{
					// Find matchings:
					mrpt::tfest::TSE3RobustParams params;
					params.ransac_minSetSize = 3;
					params.ransac_maxSetSizePct = 6.0 / corrs.size();

					mrpt::tfest::TSE3RobustResult results;
					bool register_ok = false;
					try
					{
						mrpt::tfest::se3_l2_robust(corrs, params, results);
						register_ok = true;
					}
					catch (std::exception&)
					{
						/* Cannot find a minimum number of matches, inconsistent
						 * parameters due to very reduced numberof matches,etc.
						 */
					}

					const CPose3D relativePose =
						CPose3D(results.transformation);

					str_status = mrpt::format(
						"%d corrs | inliers: %d | rel.pose: %s ",
						int(corrs.size()), int(results.inliers_idx.size()),
						relativePose.asString().c_str());
					str_status2 = string(
						results.inliers_idx.size() == 0
							? "LOST! Please, press 'r' to restart"
							: "");

					if (register_ok && std::abs(results.scale - 1.0) < 0.1)
					{
						// Seems a good match:
						if ((relativePose.norm() > KEYFRAMES_MIN_DISTANCE ||
							 std::abs(relativePose.yaw()) > KEYFRAMES_MIN_ANG ||
							 std::abs(relativePose.pitch()) >
								 KEYFRAMES_MIN_ANG ||
							 std::abs(relativePose.roll()) > KEYFRAMES_MIN_ANG))
						{
							// Accept this as a new key-frame pose ------------
							// Append new global pose of this key-frame:

							const CPose3D new_keyframe_global =
								CPose3D(*camera_key_frames_path.rbegin()) +
								relativePose;

							camera_key_frames_path.push_back(
								new_keyframe_global.asTPose());

							gl_keyframes_must_refresh = true;

							currentCamPose_wrt_last =
								CPose3D();  // It's (0,0,0) since the last
							// key-frame is the current pose!
							lastVisibleFeats = curVisibleFeats;

							cout << "Adding new key-frame: pose="
								 << new_keyframe_global << endl;

							// Update global map: append another map at a given
							// position:
							globalPtsMap.insertObservation(
								last_obs.get(), &new_keyframe_global);
							win3D.get3DSceneAndLock();
							gl_points_map->loadFromPointsMap(&globalPtsMap);
							win3D.unlockAccess3DScene();
						}
						else
						{
							currentCamPose_wrt_last = relativePose;
							// cout << "cur pose: " << currentCamPose_wrt_last
							// << endl;
						}
					}
				}
			}

			if (camera_key_frames_path.empty() || lastVisibleFeats.empty())
			{
				// First iteration:
				camera_key_frames_path.clear();
				camera_key_frames_path.push_back(TPose3D(0, 0, 0, 0, 0, 0));
				gl_keyframes_must_refresh = true;
				lastVisibleFeats = curVisibleFeats;

				// Update global map:
				globalPtsMap.clear();
				globalPtsMap.insertObservation(last_obs.get());

				win3D.get3DSceneAndLock();
				gl_points_map->loadFromPointsMap(&globalPtsMap);
				win3D.unlockAccess3DScene();
			}

			// Save the image for the next step:
			previous_image = theImg;

			// Draw marks on the RGB image:
			theImg.selectTextFont("10x20");
			{  // Tracked feats:
				theImg.drawFeatures(
					trackedFeats, TColor(0, 0, 255), SHOW_FEAT_IDS,
					SHOW_RESPONSES);
				theImg.textOut(
					3, 22,
					format("# feats: %u", (unsigned int)trackedFeats.size()),
					TColor(200, 20, 20));
			}

			// Update visualization ---------------------------------------

			// Show intensity image
			win3D.get3DSceneAndLock();
			viewInt->setImageView(theImg);
			win3D.unlockAccess3DScene();

			// Show 3D points & current visible feats, at the current camera 3D
			// pose "currentCamPose_wrt_last"
			// ---------------------------------------------------------------------
			if (last_obs->hasPoints3D)
			{
				const CPose3D curGlobalPose =
					CPose3D(*camera_key_frames_path.rbegin()) +
					currentCamPose_wrt_last;
				win3D.get3DSceneAndLock();
				// All 3D points:
				gl_points->loadFromPointsMap(&localPntsMap);
				gl_points->setPose(curGlobalPose);
				gl_cur_cam_corner->setPose(curGlobalPose);

				// Current visual landmarks:
				gl_curFeats->clear();
				for (map<TFeatureID, TPoint3D>::const_iterator it =
						 curVisibleFeats.begin();
					 it != curVisibleFeats.end(); ++it)
				{
					static double D = 0.02;
					mrpt::opengl::CBox::Ptr box =
						mrpt::make_aligned_shared<mrpt::opengl::CBox>(
							TPoint3D(-D, -D, -D), TPoint3D(D, D, D));
					box->setWireframe(true);
					box->setName(format("%d", int(it->first)));
					box->enableShowName(true);
					box->setLocation(it->second);
					gl_curFeats->insert(box);
				}
				gl_curFeats->setPose(curGlobalPose);

				win3D.unlockAccess3DScene();
				win3D.repaint();
			}

			win3D.get3DSceneAndLock();
			win3D.addTextMessage(
				-100, -20, format("%.02f Hz", thrPar.Hz), TColorf(0, 1, 1), 100,
				MRPT_GLUT_BITMAP_HELVETICA_18);
			win3D.unlockAccess3DScene();

			win3D.repaint();

			step_num++;

		}  // end update visualization:

		if (gl_keyframes_must_refresh)
		{
			gl_keyframes_must_refresh = false;
			// cout << "Updating gl_keyframes with " <<
			// camera_key_frames_path.size() << " frames.\n";
			win3D.get3DSceneAndLock();
			gl_keyframes->clear();
			for (size_t i = 0; i < camera_key_frames_path.size(); i++)
			{
				CSetOfObjects::Ptr obj =
					mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 3);
				obj->setPose(camera_key_frames_path[i]);
				gl_keyframes->insert(obj);
			}
			win3D.unlockAccess3DScene();
		}

		// Process possible keyboard commands:
		// --------------------------------------
		if (win3D.keyHit() && thrPar.pushed_key == 0)
		{
			const int key = tolower(win3D.getPushedKey());

			switch (key)
			{
				// Some of the keys are processed in this thread:
				case 'r':
					lastVisibleFeats.clear();
					camera_key_frames_path.clear();
					gl_keyframes_must_refresh = true;
					globalPtsMap.clear();
					win3D.get3DSceneAndLock();
					gl_points_map->loadFromPointsMap(&globalPtsMap);
					win3D.unlockAccess3DScene();

					break;
				case 's':
				{
					const std::string s = "point_cloud.txt";
					cout << "Dumping 3D point-cloud to: " << s << endl;
					globalPtsMap.save3D_to_text_file(s);
					break;
				}
				case 'o':
					win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
					win3D.repaint();
					break;
				case 'i':
					win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
					win3D.repaint();
					break;
				// ...and the rest in the kinect thread:
				default:
					thrPar.pushed_key = key;
					break;
			};
		}

		win3D.get3DSceneAndLock();
		win3D.addTextMessage(
			2, -30, format(
						"'s':save point cloud, 'r': reset, 'o'/'i': zoom "
						"out/in, mouse: orbit 3D, ESC: quit"),
			TColorf(1, 1, 1), 110, MRPT_GLUT_BITMAP_HELVETICA_12);
		win3D.addTextMessage(
			2, -50, str_status, TColorf(1, 1, 1), 111,
			MRPT_GLUT_BITMAP_HELVETICA_12);
		win3D.addTextMessage(
			2, -70, str_status2, TColorf(1, 1, 1), 112,
			MRPT_GLUT_BITMAP_HELVETICA_18);
		win3D.unlockAccess3DScene();

		std::this_thread::sleep_for(1ms);
	}

	cout << "Waiting for grabbing thread to exit...\n";
	thrPar.quit = true;
	thHandle.join();
	cout << "Bye!\n";
}
Esempio n. 13
0
CSetOfObjects::Ptr stock_objects::Househam_Sprayer()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	{
		CBox::Ptr cabin = mrpt::make_aligned_shared<CBox>(
			TPoint3D(0.878, 0.723, -0.12), TPoint3D(-0.258, -0.723, -1.690));
		cabin->setColor(0.7, 0.7, 0.7);
		ret->insert(cabin);
	}
	{
		CBox::Ptr back = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-0.258, 0.723, -0.72), TPoint3D(-5.938, -0.723, -1.690));
		back->setColor(1, 1, 1);
		ret->insert(back);
	}
	{
		CBox::Ptr boomAxis = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, -0.723, -1.690));
		boomAxis->setColor(0, 0, 0);
		ret->insert(boomAxis);
	}
	{
		CBox::Ptr boom1 = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, 11.277, -1.620));
		boom1->setColor(0, 1, 0);
		ret->insert(boom1);
	}
	{
		CBox::Ptr boom2 = mrpt::make_aligned_shared<CBox>(
			TPoint3D(-5.938, -0.723, -1.0), TPoint3D(-6.189, -11.277, -1.620));
		boom2->setColor(0, 1, 0);
		ret->insert(boom2);
	}
	{
		CCylinder::Ptr cyl1 =
			mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(-0.710, 0.923, -2.480, 0, 0, DEG2RAD(90)));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl2->setColor(0, 0, 0);
		cyl2->setPose(CPose3D(-3.937, 0.923, -2.480, 0, 0, DEG2RAD(90)));
		ret->insert(cyl2);
	}
	{
		CCylinder::Ptr cyl1 =
			mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl1->setColor(0, 0, 0);
		cyl1->setPose(CPose3D(-0.710, -0.423, -2.480, 0, 0, DEG2RAD(90)));
		ret->insert(cyl1);
	}
	{
		CCylinder::Ptr cyl2 =
			mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
		cyl2->setColor(0, 0, 0);
		cyl2->setPose(CPose3D(-3.937, -0.423, -2.480, 0, 0, DEG2RAD(90)));
		ret->insert(cyl2);
	}
	return ret;
}
Esempio n. 14
0
/*---------------------------------------------------------------
					RobotGiraff
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotGiraff()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
	float height = 0;

	// Base
	vector<TPoint2D> level1;
	level1.push_back(TPoint2D(0.31, 0));
	level1.push_back(TPoint2D(0.22, 0.24));
	level1.push_back(TPoint2D(-0.22, 0.24));
	level1.push_back(TPoint2D(-0.31, 0));
	level1.push_back(TPoint2D(-0.22, -0.24));
	level1.push_back(TPoint2D(0.22, -0.24));

	CPolyhedron::Ptr obj1 =
		opengl::CPolyhedron::CreateCustomPrism(level1, 0.23);
	obj1->setLocation(0, 0, height);
	height += 0.23f;
	obj1->setColor(1.0, 0.6, 0.0);
	ret->insert(obj1);

	// Electronic's cage
	vector<TPoint2D> level2;
	level2.push_back(TPoint2D(0.13, 0.1));
	level2.push_back(TPoint2D(-0.13, 0.1));
	level2.push_back(TPoint2D(-0.13, -0.1));
	level2.push_back(TPoint2D(0.13, -0.1));

	CPolyhedron::Ptr obj2 =
		opengl::CPolyhedron::CreateCustomPrism(level2, 0.45);
	obj2->setLocation(0, 0, height);
	height += 0.45f;
	obj2->setColor(1.0, 0.6, 0.2);
	ret->insert(obj2);

	// Neck
	vector<TPoint2D> level3;
	level3.push_back(TPoint2D(0.03, 0.03));
	level3.push_back(TPoint2D(-0.03, 0.03));
	level3.push_back(TPoint2D(-0.03, -0.03));
	level3.push_back(TPoint2D(0.03, -0.03));

	CPolyhedron::Ptr obj3 =
		opengl::CPolyhedron::CreateCustomPrism(level3, 0.55);
	obj3->setLocation(0, 0, height);
	height += 0.55f;
	obj3->setColor(0.6, 0.6, 0.6);
	ret->insert(obj3);

	// Screen
	vector<TPoint2D> level4;
	level4.push_back(TPoint2D(0.03, 0.11));
	level4.push_back(TPoint2D(-0.03, 0.11));
	level4.push_back(TPoint2D(-0.03, -0.11));
	level4.push_back(TPoint2D(0.03, -0.11));

	CPolyhedron::Ptr obj4 = opengl::CPolyhedron::CreateCustomPrism(level4, 0.4);
	obj4->setLocation(0, 0, height);
	height += 0.4f;
	obj4->setColor(1.0, 0.6, 0.0);
	ret->insert(obj4);

	return ret;
}
Esempio n. 15
0
/*---------------------------------------------------------------
					RobotPioneer
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotPioneer()
{
	CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

	ret->setName("theRobot");

	CSetOfTriangles::Ptr obj = mrpt::make_aligned_shared<CSetOfTriangles>();

	// Add triangles:
	CSetOfTriangles::TTriangle trian;

	trian.r[0] = trian.r[1] = trian.r[2] = 1;
	trian.g[0] = trian.g[1] = trian.g[2] = 0;
	trian.b[0] = trian.b[1] = trian.b[2] = 0;
	trian.a[0] = trian.a[1] = trian.a[2] = 1;

	trian.x[0] = 0.10f;
	trian.x[1] = -0.20f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = -0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.25f;
	trian.z[2] = 0.25f;
	obj->insertTriangle(trian);  // 0
	trian.x[0] = 0.10f;
	trian.x[1] = 0.10f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.20f;
	trian.z[2] = 0.25f;
	obj->insertTriangle(trian);  // 1

	// trian.r = 0.9f; trian.g = 0; trian.b = 0; trian.a = 1;

	trian.x[0] = 0.10f;
	trian.x[1] = 0.10f;
	trian.x[2] = 0.10f;
	trian.y[0] = -0.10f;
	trian.y[1] = -0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.05f;
	trian.z[1] = 0.20f;
	trian.z[2] = 0.20f;
	obj->insertTriangle(trian);  // 2
	trian.x[0] = 0.10f;
	trian.x[1] = 0.10f;
	trian.x[2] = 0.10f;
	trian.y[0] = -0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.05f;
	trian.z[1] = 0.05f;
	trian.z[2] = 0.20f;
	obj->insertTriangle(trian);  // 3

	trian.x[0] = -0.20f;
	trian.x[1] = -0.20f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = -0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.05f;
	trian.z[1] = 0.25f;
	trian.z[2] = 0.25f;
	obj->insertTriangle(trian);  // 2b
	trian.x[0] = -0.20f;
	trian.x[1] = -0.20f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.05f;
	trian.z[1] = 0.05f;
	trian.z[2] = 0.25f;
	obj->insertTriangle(trian);  // 3b

	// trian.r = 0.8f; trian.g = 0; trian.b = 0; trian.a = 1;
	trian.x[0] = 0.10f;
	trian.x[1] = -0.20f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = -0.10f;
	trian.y[2] = -0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.25f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 4
	trian.x[0] = 0.10f;
	trian.x[1] = 0.10f;
	trian.x[2] = -0.20f;
	trian.y[0] = -0.10f;
	trian.y[1] = -0.10f;
	trian.y[2] = -0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.05f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 5

	trian.x[0] = 0.10f;
	trian.x[1] = -0.20f;
	trian.x[2] = -0.20f;
	trian.y[0] = 0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.25f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 6
	trian.x[0] = 0.10f;
	trian.x[1] = 0.10f;
	trian.x[2] = -0.20f;
	trian.y[0] = 0.10f;
	trian.y[1] = 0.10f;
	trian.y[2] = 0.10f;
	trian.z[0] = 0.20f;
	trian.z[1] = 0.05f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 7

	trian.r[0] = trian.r[1] = trian.r[2] = 0.05f;
	trian.g[0] = trian.g[1] = trian.g[2] = 0.05f;
	trian.b[0] = trian.b[1] = trian.b[2] = 0.05f;
	trian.a[0] = trian.a[1] = trian.a[2] = 1;

	trian.x[0] = 0.00f;
	trian.x[1] = 0.00f;
	trian.x[2] = 0.05f;
	trian.y[0] = 0.11f;
	trian.y[1] = 0.11f;
	trian.y[2] = 0.11f;
	trian.z[0] = 0.00f;
	trian.z[1] = 0.10f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 8
	trian.x[0] = 0.00f;
	trian.x[1] = 0.00f;
	trian.x[2] = -0.05f;
	trian.y[0] = 0.11f;
	trian.y[1] = 0.11f;
	trian.y[2] = 0.11f;
	trian.z[0] = 0.00f;
	trian.z[1] = 0.10f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 9

	trian.x[0] = 0.00f;
	trian.x[1] = 0.00f;
	trian.x[2] = 0.05f;
	trian.y[0] = -0.11f;
	trian.y[1] = -0.11f;
	trian.y[2] = -0.11f;
	trian.z[0] = 0.00f;
	trian.z[1] = 0.10f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 10
	trian.x[0] = 0.00f;
	trian.x[1] = 0.00f;
	trian.x[2] = -0.05f;
	trian.y[0] = -0.11f;
	trian.y[1] = -0.11f;
	trian.y[2] = -0.11f;
	trian.z[0] = 0.00f;
	trian.z[1] = 0.10f;
	trian.z[2] = 0.05f;
	obj->insertTriangle(trian);  // 11

	ret->insert(obj);

	return ret;
}