structured_light_scan.cpp

#include <dirent.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <stdio.h>
#include <string>

using namespace std;

int b = 72;
int f = 22;

double left_theta = 1.5708;
double right_theta = 1.01129;
double left_alpha = 0.23022;
double right_alpha = 0.33389;
int frame_count = 96;

int image_size_x = 1280;
int image_size_y = 720;

int sub_sampling_rate = 8;
int expected_x_deviation = 100;

static void scan_dir(string dir_name);
static void scan_video(string filename);
static void scan_image(string filename, int scan_number, double (&z_array)[1280][720]);
static double z_triangulation(int x, int y, int scan_number);
static double translate(double value, double left_min, double left_max, double right_min, double right_max);
static void output_pcd(double (&z_array)[1280][720]);
static bool is_white(int r, int g, int b);
static void print_divider();

void scan_dir(string dir_name) {
	printf("Processing scans in directory: %s\n", dir_name.c_str());
	print_divider();

	char *a = (char*) dir_name.c_str();
	DIR* dirp = opendir(a);
	struct dirent *ent;

	int scan_number = 0;
	if (dirp != NULL) {
	  while ((ent = readdir (dirp)) != NULL) {
	  	string filename = ent->d_name;
	  	size_t found = filename.find("png");
	  	if (found != string::npos) {
	  		scan_image(filename, scan_number, z_array);
	  		scan_number++;
	  	}
	  }
	  closedir (dirp);
	}

	printf("%d files", scan_number);
}

void scan_video(string filename) {

}

void scan_image(string filename, int scan_number, double (&z_array)[1280][720]) {

}

double z_triangulation(int x, int y, int scan_number) {
	y = image_size_y - y;
	double theta = translate(scan_number, 0.0, frame_count - 1, left_theta, right_theta);
	double alpha = translate(scan_number, 0.0, frame_count - 1, left_alpha, right_alpha);
	double z = b * (sin(theta) / sin(alpha + theta));

	return z * (-f);
}

double translate(double value, double left_min, double left_max, double right_min, double right_max) {
	double left_span = left_max - left_min;
	double right_span = right_max - right_min;

	double value_scaled = (value - left_min) - left_span;

	return right_min + (value_scaled * right_span);
}

void output_pcd(double (&z_array)[1280][720]) {
	int point_count = 0;
	for (int x = 0; x < image_size_x; x++) {
		for (int y = 0; y < image_size_y; y++) {
			if (z_array[x][y] != 0) {
				point_count++;
			}
		}
	}

	ofstream out_file;
	out_file.open("output.pcd");

	char count_string[point_count / 10 + 1];
	sprintf(count_string, "%d", point_count);

	out_file << "# .PCD v.7 -- file generated by structured_light_scan.cpp\n";
	out_file << "VERSION .7\n";
	out_file << "FIELDS x y z\n";
	out_file << "SIZE 8 8 8\n";
	out_file << "TYPE F F F\n";
	out_file << "COUNT 1 1 1\n";
	out_file << "WIDTH ";
	out_file << count_string;
	out_file << "\n";
	out_file << "HEIGHT 1\n";
	out_file << "VIEWPOINT 0 0 0 1 0 0 0\n";
	out_file << "POINTS ";
	out_file << count_string;
	out_file << "\n";
	out_file << "DATA ascii\n";

	for (int x = 0; x < image_size_x; x++) {
		for (int y = 0; y < image_size_y; y++) {
			if (z_array[x][y] != 0) {
				char x_string[x / 10 + 1];
				sprintf(x_string, "%d", x);
				out_file << x_string;

				out_file << " ";

				int adjusted_y = image_size_y - y;
				char y_string[adjusted_y / 10 + 1];
				sprintf(y_string, "%d", adjusted_y);
				out_file << y_string;

				out_file << " ";

				char z_string[6];
				sprintf(z_string, "%.4f", z_array[x][y]);
				out_file << z_string;

				out_file << "\n";
			}
		}
	}

	out_file.close();
}

bool is_white(int r, int g, int b) {
	if (r == 255 && g == 255 && b == 255) {
		return true;
	} else {
		return false;
	}
}

void print_divider() {
	printf("-----------------------------------------------------");
}

int main() {
	scan_dir("/Volumes/theoc/evan/Google Drive/scan_lines/");
	return 0;
}