% David E. Haddad, J Ramon Arrowsmith -- 11/10/2010

% This script will take an unreferenced xyz dataset, such as a terrestrial 
% laser scanning (TLS) point cloud and place it in a Cartesian global 
% coordinate system (UTM). The result is a referenced xyz point cloud in 
% ASCII format.

% NB: Depending on your computer's RAM capacity, it is not advised to run
% this script on xyz datasets greater than ~1 Gb. Otherwise, you will
% likely run into memory problems. If your original dataset is greater than
% ~1 Gb, we recommend using the Linux (Korn) version of this utility that
% is available from http://lidar.asu.edu or the OpenTopography Tool
% Repository at www.opentopography.org

% Accepted input file formats:
% ASCII (.txt)

% Output file format:
% ASCII (.txt)


%%%%%%%%%%%%%%%%%%%%%%%%%   USER-DEFINED INPUTS   %%%%%%%%%%%%%%%%%%%%%%%%%

% (1) The ungeoreferenced dataset's file name in the single quotation marks
% below. Make sure to include the file extension!
fid = fopen('original_TLS_data.txt');
% (1.5) Define the file delimiter
original_TLS_data = textscan(fid, '%f %f %f', 'delimiter', ',');
fclose(fid);

% (2) The total number of points in the ungeoreferenced dataset.
num_points = 4977725;

% (3) Cartesian x and y (eastings and northings if using UTM) coordinates of
% at least one of the unreferenced points. This point can be a common point
% between an ungeoreferenced TLS dataset and a georeferenced airborne laser
% scanning (ALS) dataset.
first_common_TLS_easting = -4.718;
first_common_TLS_northing = 21.964;
first_common_ALS_easting = 369793.400024;
first_common_ALS_northing = 3829925.039978;

% (4) An elevation correction parameter. This should be a number that is the
% difference between the elevations of the ungeoreferenced points and the
% georeferenced points. Because we are assuming a rigid body rotation and
% translation, a single elevation correction suffices.
elevation_correction = 1550.009;

% (5) A rotation angle (in degrees) with which to rotate the ungeoreferenced
% dataset. You will need to experiment with this to see which is the best
% rotation angle that will place the ungeoreferenced dataset in a global 
% coordinate system.
rotation_angle_degs = -100;
   % Automatically converted to radians:
rotation_angle_rads = rotation_angle_degs.*pi./180;

%%%%%%%%%%%%%%%%%%%%%%%%  END OF USER-DEFINED INPUTS  %%%%%%%%%%%%%%%%%%%%%


% Here we go...

clear all; clf

% Assign the first, second, and third columns to easting, northing, and
% elevation.
TLS_data = zeros(num_points,3);
TLS_easting = original_TLS_data{1};
TLS_northing = original_TLS_data{2};
TLS_elevations = original_TLS_data{3};

% Plot the original TLS dataset
figure(1); clf; hold on; axis equal; plot(TLS_easting, TLS_northing, 'k.');
title('Plot of orginal coordinates'); xlabel('Easting (m)'); ylabel('Northing (m)')

% Translate the TLS data to a local origin (0,0) and vectorize them.
TLS_easting_translated = TLS_easting - first_common_TLS_easting;
TLS_northing_translated = TLS_northing - first_common_TLS_northing;
TLS_translated_coordinates = [TLS_easting_translated TLS_northing_translated];

% Plot the translated TLS points.
figure(2); clf; hold on; axis equal; title('Plot of translated coordinates')
xlabel('Easting (m)'); ylabel('Northing (m)')
plot(TLS_easting_translated, TLS_northing_translated,'k.'); plot(0,0,'ro')

% Now let's do some rotations!
% Preallocate memory so this script runs efficiently (learn to love your vectors!!)
TLS_easting_prime = zeros(num_points,1);
TLS_northing_prime = zeros(num_points,1);

% Vectorize the computation interval (aka the "i" parameter in a "for" loop)
i = 1:num_points;

% BEHOLD, THE ROTATION EQUATION THAT TAKETH AWAY THE SINS OF THE WORLD!
TLS_easting_prime(i) = TLS_translated_coordinates(i,1).*cos(rotation_angle_rads) + TLS_translated_coordinates(i,2).*sin(rotation_angle_rads);
TLS_northing_prime(i) = -TLS_translated_coordinates(i,1).*sin(rotation_angle_rads) + TLS_translated_coordinates(i,2).*cos(rotation_angle_rads);

% Plot the rotated TLS points
figure(3); clf; hold on; axis equal;
title('Plot of coordinates rotated about a local origin');
xlabel('Easting prime (m)'); ylabel('Northing prime (m)')
plot(TLS_easting_prime(i),TLS_northing_prime(i),'k.'); plot(0,0,'ro')

% Translate the rotated TLS points to the UTM coordinates of the ALS
% dataset.
UTM_TLS_easting(i) = TLS_easting_prime(i) + first_common_ALS_easting;
UTM_TLS_northing(i) = TLS_northing_prime(i) + first_common_ALS_northing;

% Assign the elevations.
UTM_TLS_data=zeros(num_points,3);

% Define each column and vectorize them.
UTM_TLS_data(:,1) = UTM_TLS_easting(i);
UTM_TLS_data(:,2) = UTM_TLS_northing(i);
UTM_TLS_data(:,3) = TLS_elevations + elevation_correction;
rotated_translated_data = [UTM_TLS_data(:,1) UTM_TLS_data(:,2) UTM_TLS_data(:,3)];

% Plot the rotated and translated TLS dataset in UTM
figure(4); clf; hold on; axis equal;
title('Plot of TLS coordinates in UTM');
xlabel('Easting prime (m)'); ylabel('Northing prime (m)');
plot(UTM_TLS_easting(i),UTM_TLS_northing(i),'k.')

% Write the new TLS UTM coordinates to a text file.
fid = fopen('UTM_TLS_coordinates.txt','w+t'); 
fclose(fid);
dlmwrite('UTM_TLS_coordinates.txt',rotated_translated_data(:,1:3),'-append','precision',10);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  END OF SCRIPT  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This is the "for" loop that I had initially tried to use... but MATLAB is
% *much* more efficient at processing data in vectorized form! Vectors
% save the day once again!
%
% for i = 1:length(TLS_translated_coordinates)
%         TLS_easting_prime(i) = TLS_translated_coordinates(i,1).*cos(rotation_angle_rads) + TLS_translated_coordinates(i,2).*sin(rotation_angle_rads);
%         TLS_northing_prime(i) = -TLS_translated_coordinates(i,1).*sin(rotation_angle_rads) + TLS_translated_coordinates(i,2).*cos(rotation_angle_rads);
%         %[TLS_easting_prime(i), TLS_northing_prime(i)] = rotater(TLS_translated_coordinates(i,1),TLS_translated_coordinates(i,2),rotation_angle);
%         plot(TLS_easting_prime(i), TLS_northing_prime(i),'b+')
%         %UTM_TLS_easting(i) = TLS_easting_prime(i) + first_ALS_easting;
%         %UTM_TLS_northing(i) = TLS_northing_prime(i) + first_ALS_northing;
%         %UTM_TLS_data = [UTM_TLS_easting(i) UTM_TLS_northing(i)];
% end