example of file io in C++, converts a .pcd file to a vector of PointXYZ

cpp_file_io_pcd.cpp

#include "stdafx.h"
#include "util.h"
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <string>
#include <map>
#include <vector>
#include <cstdio>
#include <sstream>

#include <librealsense/rs.hpp>
#include <opencv2/core.hpp>
#include <librealsense/rs.hpp>

//#include <opencv2/core.hpp>
//#include <opencv2/highgui.hpp>
//#include <opencv2/calib3d.hpp>
// Also include GLFW to allow for graphical display
#define GLFW_INCLUDE_GLU
#include <GLFW/glfw3.h>

void initialize_s_map()
{
	s_mapSV["VERSION"] = VERSION;
	s_mapSV["FIELDS"] = FIELDS;
	s_mapSV["SIZE"] = SIZE;
	s_mapSV["TYPE"] = TYPE;
	s_mapSV["COUNT"] = COUNT;
	s_mapSV["WIDTH"] = WIDTH;
	s_mapSV["HEIGHT"] = HEIGHT;
	s_mapSV["VIEWPOINT"] = VIEWPOINT;
	s_mapSV["POINTS"] = POINTS;
	s_mapSV["DATA"] = DATA;
}


vector<PointXYZRGB> pcdReader(string filename)
{
	initialize_s_map();
	ifstream pcd(filename, ios::in); // open the file for input mode
	string line; // buffer to store the line input
	int width, height, color_type; // width, height and no. of points in the cloud
	long int points, rgb; // no. of points
	vector<PointXYZRGB> output;

	if (pcd.is_open()) try
	{
		// process the header of the file up to DATA
		bool header_parsed = false;
		while (getline(pcd, line))
		{	
			if (!header_parsed)
			{
				cout << line.substr(0, line.find_first_of(' ')) << '\n';
				switch (s_mapSV[line.substr(0, line.find_first_of(' '))])
				{
				case '#': case VERSION: case SIZE: case COUNT: case VIEWPOINT: case TYPE: { break; } // not using these values
				case FIELDS:
				{
					int color_type;
					// last word in string, either "rgb" or "rgbd"
					string pc_type = line.substr(line.find_last_of(' ')+1, line.size());
					// cout << pc_type << '\n';
					if (pc_type.compare("rgba")){ color_type = RGBA; }
					else
						if (pc_type.compare("rgb")){ color_type = RGB; }
						else
							//cout << ("cloud type of " + pc_type + " not supported \n");
							throw std::invalid_argument("cloud type of " + pc_type + " not supported \n");
					break;
				}
				case WIDTH:
				{
					string s_width = line.substr(line.find_last_of(' ')+1, line.size());
					width = stoi(s_width);
					break;
				}
				case HEIGHT:
				{
					string s_height = line.substr(line.find_last_of(' ')+1, line.size());
					height = stoi(s_height);
					break;
				}
				case POINTS:
				{
					string last_word = line.substr(line.find_last_of(' ')+1, line.size());
					points = stol(last_word);
					break;
				}
				case DATA:
				{
					string last_word = line.substr(line.find_last_of(' ')+1, line.size());
					if (last_word.compare("ascii") != 0) 
					{ 
						cout << "only ascii files supported but " + last_word + " supplied \n";
						throw std::invalid_argument("only ascii files supported but " + last_word + " supplied");
					}
					header_parsed = true;
					getline(pcd, line);
					break;
				}
				default:
					break;
				}
			}
			if (header_parsed)
			{
				if (line.compare("DATA ascii") == 0){}
				else
				{
					string point_data;
					istringstream ss(line);

					float xyz_coord[3];
					for (int i = 0; i < 3; ++i)
					{
						getline(ss, point_data, ' '); // delimited by a tab
						// cout << point_data;
						xyz_coord[i] = (stof(point_data));
					}
					getline(ss, point_data); // last element of the line, RGB value
					// cout << point_data;
					rgb = stol(point_data);
					// cout << rgb << '\n';
					output.push_back(PointXYZRGB{ { xyz_coord[0], xyz_coord[1], xyz_coord[2] }, rgb });
				}
			}	
		}
		cout << "width: " << width << '\n'
			<< "height: " << height << '\n'
			<< "points: " << points << '\n';
		pcd.close();
		return output;
	}
	catch (std::invalid_argument& e)
	{
		cout << e.what() << '\n';
	}
	else cout << "unable to open file";
	
}

//cv::Mat float3ToMat(vector<PointXYZRGB> cloud)
//{
//	long int cloud_length = cloud.size();
//	cv::Mat out(cloud_length, 3, CV_32F);
//	for (long int i = 0; i < cloud_length; ++i)
//	{
//		out.at<float>(i, 0) = cloud[i].xyz.x;
//		out.at<float>(i, 1) = cloud[i].xyz.y;
//		out.at<float>(i, 2) = cloud[i].xyz.z;
//		//out.row(i).col(0) = cloud[i].xyz.x;
//		//out.row(i).col(1) = cloud[i].xyz.y;
//		//out.row(i).col(2) = cloud[i].xyz.z;
//	}
//	return out;
//}

rs::device* rs_setup()
{
	try
	{
		// setting up the camera 
		rs::log_to_console(rs::log_severity::warn);
		rs::context ctx;
		if (ctx.get_device_count())
			printf("There are %d connected RealSense devices.\n", ctx.get_device_count());

		rs::device *cam = ctx.get_device(0); // get the first camera
		printf("\nUsing the first device connected (%s) \n", cam->get_name());
		printf("    Serial number: %s\n", cam->get_serial());
		printf("    Firmware version: %s\n", cam->get_firmware_version());

		// enable both color and depth streams 
		// TODO find out what best_quality means
		cam->enable_stream(rs::stream::color, rs::preset::best_quality);
		cam->enable_stream(rs::stream::depth, rs::preset::best_quality);
		cam->start();

		return cam;
	}
	catch (const rs::error &e)
	{
		// Method calls against librealsense objects may throw exceptions of type rs::error
		printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
		printf("    %s\n", e.what());
		return nullptr;
	}
}

double yaw, pitch, lastX, lastY; int ml;
static void on_mouse_button(GLFWwindow * win, int button, int action, int mods)
{
	if (button == GLFW_MOUSE_BUTTON_LEFT) ml = action == GLFW_PRESS;
}
static double clamp(double val, double lo, double hi) { return val < lo ? lo : val > hi ? hi : val; }
static void on_cursor_pos(GLFWwindow * win, double x, double y)
{
	if (ml)
	{
		yaw = clamp(yaw - (x - lastX), -120, 120);
		pitch = clamp(pitch + (y - lastY), -80, 80);
	}
	lastX = x;
	lastY = y;
}

int render(vector<PointXYZRGB> cloud) try
{
	// Turn on logging. We can separately enable logging to console or to file, and use different severity filters for each.
	rs::log_to_console(rs::log_severity::warn);
	//rs::log_to_file(rs::log_severity::debug, "librealsense.log");

	// Create a context object. This object owns the handles to all connected realsense devices.
	
	// Open a GLFW window to display our output
	glfwInit();
	GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", nullptr, nullptr);
	glfwSetCursorPosCallback(win, on_cursor_pos);
	glfwSetMouseButtonCallback(win, on_mouse_button);
	glfwMakeContextCurrent(win);
	while (!glfwWindowShouldClose(win))
	{
		// Wait for new frame data
		glfwPollEvents();
		

		

		// Set up a perspective transform in a space that we can rotate by clicking and dragging the mouse
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		gluPerspective(60, (float)1280 / 960, 0.01f, 20.0f);
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0);
		glTranslatef(0, 0, +0.5f);
		glRotated(pitch, 1, 0, 0);
		glRotated(yaw, 0, 1, 0);
		glTranslatef(0, 0, -0.5f);

		// We will render our depth data as a set of points in 3D space
		glPointSize(2);
		glEnable(GL_DEPTH_TEST);
		glBegin(GL_POINTS);

		for (long i = 0; i < cloud.size(); ++i)
		{
			glColor3ub(255, 255, 255);
			// Emit a vertex at the 3D location of this depth pixel
			glVertex3f(cloud[i].xyz.x, cloud[i].xyz.y, cloud[i].xyz.z);
			}
		}
		glEnd();
		glfwSwapBuffers(win);
		return EXIT_SUCCESS;
	}
catch (const rs::error & e)
{
	// Method calls against librealsense objects may throw exceptions of type rs::error
	printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
	printf("    %s\n", e.what());
	return EXIT_FAILURE;
}