Exemplo n.º 1
0
void Projector::showRectangle(bool gpuView)
{
	indices.resize(480);
	for (int i = 0; i < 480; i++) indices[i].resize(640);

	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 0, 0);
		//setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> wrldSrc;
		std::vector<cv::Point3f> plnSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		
		for (int i = 0; i<512; i++)
		{
			for (int j = 0; j<424; j++)
			{
				float x = 0, y = 0, z = 0, color = 0;
				registration->getPointXYZRGB(&undistorted, &registered,
					i, j,
					x, y, z, color);

				if (z>0.5 && z<2.1)
				{
					x = static_cast<float>(x + right / ((double)640.0)); //////////TO-DO fix that
					y = static_cast<float>(y + up / ((double)480.0));

					x -= 0.5;
					y -= 0.5;
					double PI = 3.14159265;
					x = static_cast<float>(std::cos(rotX * PI / 180) * x - std::sin(rotX * PI / 180) * y);
					y = static_cast<float>(std::sin(rotX * PI / 180) * x + std::cos(rotX * PI / 180) * y);

					x += 0.5;
					y += 0.5;

					wrldSrc.push_back(cv::Point3f(x * 100,
						y * 100,
						z * 100));
				}
			}
		}
		
		PlaneData pln = findRectangle(registration, &undistorted, &registered);
		if (wrldSrc.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(wrldSrc);
			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(100, 100, 150, 100));
				}
			}
			if (pln.points.size() > 0) {
				projected = projectPoints(pln.points);
				cv::Mat cont = cv::Mat(480, 640, CV_8UC1, cv::Scalar::all(0));

				for (int i = 0; i < projected.size(); i++)
				{
					if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {
						
						cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
						ROI.setTo(cv::Scalar(250, 100, 100, 100));
						cont.at<uchar>(static_cast<int>(480 - projected[i].x), static_cast<int>(projected[i].y), 0) = 255;
						indices[static_cast<int>(480 - projected[i].x)][static_cast<int>(projected[i].y)] = i;
					}
				}
				vector<vector<cv::Point> > contours;
				vector<cv::Vec4i> hierarchy;
				cv::GaussianBlur(cont, cont, cv::Size(7, 7), 5, 11);
				findContours(cont, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));

				vector<vector<cv::Point> > contours_poly(contours.size());
				vector<cv::Rect> boundRect(contours.size());
				vector<cv::Point2f>center(contours.size());
				vector<float>radius(contours.size());
				int nPoly;
				for (int i = 0; i < contours.size(); i++)
				{
					cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 10, true);
					nPoly = contours_poly[i].size();
					
				}

				for (int i = 0; i< contours.size(); i++)
				{
					drawContours(board, contours_poly, 0, cv::Scalar(0, 255, 0), 5);
				}

			}
			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;
			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			right = 0;
			up = 0;
			rotX = 0;
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
}
Exemplo n.º 2
0
void Projector::objProjectionOffline(std::string objPath, std::string objName, bool gpuView)
{
	std::cout << "Camera init: ";
	objObject obj(objPath, objName);
	obj.loadData();
	cout << "DONE\n";
	cv::namedWindow("objTest", CV_WINDOW_NORMAL);
	cv::moveWindow("objTest", 0, 0);
	indices.resize(480);
	for (int i = 0; i < 480; i++) indices[i].resize(640);

	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	cv::Vec3f prevNormal(-1, -1, -1);
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 200, 200);
		//setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> plnSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		PlaneData pln = findRectangle(registration, &undistorted, &registered);

		if (pln.points.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(pln.points);
			cv::Mat cont = cv::Mat(480, 640, CV_8UC1, cv::Scalar::all(0));

			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(250, 100, 100, 100));
					cont.at<uchar>(static_cast<int>(480 - projected[i].x), static_cast<int>(projected[i].y), 0) = 255;
					plnSrc.push_back(pln.points[i]);
				}
			}
			vector<vector<cv::Point> > contours;
			vector<cv::Vec4i> hierarchy;
			cv::GaussianBlur(cont, cont, cv::Size(7, 7), 5, 11);
			findContours(cont, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));

			vector<vector<cv::Point> > contours_poly(contours.size());
			vector<cv::Rect> boundRect(contours.size());
			vector<cv::Point2f>center(contours.size());
			vector<float>radius(contours.size());

			for (int i = 0; i < contours.size(); i++)
			{
				cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 10, true);
			}

			for (int i = 0; i < contours.size(); i++)
			{
				drawContours(board, contours_poly, 0, cv::Scalar(0, 255, 0), 5);
			}


			cv::Mat data_pts = cv::Mat(300, 3, CV_64FC1);
			cv::Vec3f normal(0, 0, 0);
			int jump = plnSrc.size() / 300;
			for (int i = 0; i < 100; i++) {
				data_pts.at<double>(i, 0) = plnSrc[i*jump].x;
				data_pts.at<double>(i, 1) = plnSrc[i*jump].y;
				data_pts.at<double>(i, 2) = plnSrc[i*jump].z;
				data_pts.at<double>(i + 100, 0) = plnSrc[(i + 100)*jump].x;
				data_pts.at<double>(i + 100, 1) = plnSrc[(i + 100)*jump].y;
				data_pts.at<double>(i + 100, 2) = plnSrc[(i + 100)*jump].z;
				data_pts.at<double>(i + 200, 0) = plnSrc[(i + 200) *jump].x;
				data_pts.at<double>(i + 200, 1) = plnSrc[(i + 200)*jump].y;
				data_pts.at<double>(i + 200, 2) = plnSrc[(i + 200)*jump].z;
			}

			cv::PCA pca_analysis(data_pts, cv::Mat(), CV_PCA_DATA_AS_ROW);
			cv::Vec3f cntr = cv::Vec3f((pca_analysis.mean.at<double>(0, 0)),
				(pca_analysis.mean.at<double>(0, 1)),
				(pca_analysis.mean.at<double>(0, 2)));

			vector<cv::Point3f> eigen_vecs(2);
			vector<double> eigen_val(2);
			for (int i = 0; i < 2; ++i)
			{
				eigen_vecs[i] = cv::Point3f(pca_analysis.eigenvectors.at<double>(i, 0),
					pca_analysis.eigenvectors.at<double>(i, 1),
					pca_analysis.eigenvectors.at<double>(i, 2));
				eigen_val[i] = pca_analysis.eigenvalues.at<double>(0, i);
			}
			cv::Vec3f p1 = cv::Vec3f((eigen_vecs[0].x * eigen_val[0]), (eigen_vecs[0].y * eigen_val[0]), (eigen_vecs[0].z * eigen_val[0]));
			cv::Vec3f p2 = cv::Vec3f((eigen_vecs[1].x * eigen_val[1]), (eigen_vecs[1].y * eigen_val[1]), (eigen_vecs[1].z * eigen_val[1]));
			normal = p1.cross(p2);
			normal = cv::normalize(normal);
			//pln.center = cntr;

			pln.normal = normal;
			obj.setCamera(cv::Point3f(pln.center.x, -pln.center.y, -pln.center.z + 150),
				cv::Vec3f(pln.normal[0], pln.normal[1], pln.normal[2]));

			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		cv::Mat im = obj.render();
		cv::imshow("objTest", im);
		//}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			//right = 0;
			//up = 0;
			//rotX = 0;
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
	cv::destroyWindow("objTest");
}
Exemplo n.º 3
0
void Projector::reproject(bool gpuView)
{
	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 1200, 0);
		cv::setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> wrldSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		for (int i = 0; i<512; i++)
		{
			for (int j = 0; j<424; j++)
			{
				float x = 0, y = 0, z = 0, color = 0;
				registration->getPointXYZRGB(&undistorted, &registered,
					i, j,
					x, y, z, color);

				if (z>0.5 && z<1.7)
				{
					x = static_cast<float>(x + right / ((double)640.0)); //////////TO-DO fix that
					y = static_cast<float>(y + up / ((double)480.0));

					x -= 0.5;
					y -= 0.5;
					double PI = 3.14159265;
					x = static_cast<float>(std::cos(rotX * PI / 180) * x - std::sin(rotX * PI / 180) * y);
					y = static_cast<float>(std::sin(rotX * PI / 180) * x + std::cos(rotX * PI / 180) * y);

					x += 0.5;
					y += 0.5;

					wrldSrc.push_back(cv::Point3f(x * 100,
						y * 100,
						z * 100));
				}
			}
		}
		
		if (wrldSrc.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(wrldSrc);
			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(100, 100, 150, 100));
				}
			}
			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
}
Exemplo n.º 4
0
void Projector::ctProjection(std::string ctFilePath, int startPoint, int xDim, int yDim, int zDim, bool PNG)
{
	SimpleViewer viewer;
	CTObject ctObject(ctFilePath, xDim, yDim, zDim, startPoint);
	if(PNG)
		ctObject.readDataPNG();
	else
		ctObject.readData();
	libfreenect2::FrameMap frames;
	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(0));
	//cv::namedWindow("CTviewer", CV_WINDOW_NORMAL);
	//cv::moveWindow("CTviewer", 00, 0);
	//cv::setWindowProperty("CTviewer", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	{
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	libfreenect2::Frame *rgb;
	libfreenect2::Frame *depth;
	
	(_listener)->waitForNewFrame(frames);
	rgb = frames[libfreenect2::Frame::Color];
	depth = frames[libfreenect2::Frame::Depth];
	registration->undistortDepth(depth, &undistorted);

	while (!shutdown)
	{
		std::thread *nextFrame = new std::thread[1];
		/*(_listener)->waitForNewFrame(frames);
		rgb = frames[libfreenect2::Frame::Color];
		depth = frames[libfreenect2::Frame::Depth];

		registration->undistortDepth(depth, &undistorted);*/
		//registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		//cv::Mat frame = frameToMat("registered", &registered);
		cv::Mat depthFrame;// = frameToMat("depth", depth);
		//cv::Mat board(424, 512, CV_8UC4, cv::Scalar::all(0));
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(0));

		//depthFrame *= 0.001;
		cv::Mat temp_board(480, 640, CV_16UC3, cv::Scalar::all(0));

		int parts = 128;
		int jump = 512 / parts;
		
		int minX = 999;
		int maxX = -1;
		int minY = 999;
		int maxY = -1;
		std::thread *tt = new std::thread[parts];

		for (int i = 0; i < parts; ++i) {
			tt[i] = std::thread(tst, registration, &undistorted, &registered, jump * i, jump + jump * i, &board, &ctObject,
				right, up, _mr, _mt, _cam, _pro, &depthFrame, &temp_board, &minX, &maxX, &minY, &maxY);
		}
		
		for (int i = 0; i < parts; ++i)
			tt[i].join();
		//_listener->release(frames);
		nextFrame[0] = std::thread(loadNextFrame, _listener, &frames, rgb, depth, registration, &undistorted);

		int parts2 = 8;
		int jump2 = 512 / parts2;
		for (int ii = 0; ii < 3; ii++) {
			for (int i = 0; i < parts2; ++i) {
				tt[i] = std::thread(tt2, &temp_board, jump2 * i, jump2 + jump2 * i, cv::Size(3, 3), minX, maxX, minY, maxY);
			}

			for (int i = 0; i < parts2; ++i)
				tt[i].join();
		}

		
		//for (int i = 0; i < 3; i++)
			//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(3, 3));

		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(3, 3));
		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(7, 7));
		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(7, 7));
		
		//cv::medianBlur(temp_board, temp_board, 5); // TO-DO check if needed
		//cv::GaussianBlur(temp_board, temp_board, cv::Size(3, 3), 0, 1);
		//int parts3 = 16;
		//int jump3 = 480 / parts3;
		//std::thread *tt2 = new std::thread[parts];
		//for (int i = 0; i < parts3; ++i) {
		//	tt2[i] = std::thread(tt3, &board, &temp_board, &ctObject, jump3 * i, jump3 + jump3 * i);
		//	//std::cout << jump3 * i << " " << jump3 + jump3 * i << "\n";
		//}

		//for (int i = 0; i < parts; ++i)
		//	tt2[i].join();
		//std::cout << "zuoooooo\n";
		/*for (int i = 0; i < 480; i++)
		{
			for (int j = 0; j < 640; j++)
			{
				cv::Mat ROI = board(cv::Rect(j, i, 1, 1));
				cv::Vec3s v = temp_board.at<cv::Vec3s>(i, j);
				unsigned char value = ctObject.at(v[0], v[1], v[2]);
				ROI.setTo(cv::Scalar(value, value, value, 100));
			}
		}*/

		
		//cv::imshow("CTviewer", board);
		
		{
			libfreenect2::Frame b(640, 480, 4);
			b.data = board.data;
			viewer.addFrame("RGB", &b);
			shutdown = shutdown || viewer.render();
		}

		//_listener->release(frames);
		nextFrame[0].join();
		delete[] tt;
		delete[] nextFrame;
		
		//delete[] tt2;
		/*{
			int op = cv::waitKey(1);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				shutdown = true;
				cv::destroyWindow("CTviewer");
			}
		}*/
	}


}