├── README.md ├── alignment ├── PoseTrajectoryAligner6Dof.m ├── PoseTrajectoryAlignerBase.m ├── PoseTrajectoryAlignerFirstPose.m ├── PositionTrajectoryAligner4Dof.m ├── PositionTrajectoryAligner6Dof.m ├── PositionTrajectoryAligner7Dof.m └── PositionTrajectoryAlignerBase.m ├── backend ├── conversions │ ├── quat2rot.m │ ├── rot2quat.m │ ├── ypr2quat.m │ └── ypr2rot.m ├── thirdparty │ └── absor.m └── transform_tools │ ├── k_quat_inv.m │ ├── k_quat_mag.m │ ├── k_quat_mult.m │ ├── k_quat_norm.m │ ├── k_quat_rotate.m │ ├── k_tf_identity.m │ ├── k_tf_inv.m │ ├── k_tf_mult.m │ ├── k_tf_pos.m │ ├── k_tf_quat.m │ └── k_tf_transform.m ├── data_loaders └── RosDataHelper.m ├── examples └── rtk_to_leica │ ├── align_rtk_to_leica.m │ └── rtk_enu_and_leica.bag ├── trajectories ├── OrientationTrajectory.m ├── PositionTrajectory.m ├── PositionTrajectoryWithCovariance.m ├── TrajectoryHelper.m └── TransformationTrajectory.m └── types ├── LinearTransformationBase.m ├── OrientationQuaternion.m ├── SimTransformation.m └── Transformation.m /README.md: -------------------------------------------------------------------------------- 1 | # matlab_trajectory_tools 2 | 3 | Tools for enabling quick display and analysis of trajectories and transformations in Matlab. Contains elements of mbosse_tools and mav_ransform_tools however offers an intuitive object orientated interface, adiditional functionality and per-object plotting functions allowing for very quick analysis. 4 | -------------------------------------------------------------------------------- /alignment/PoseTrajectoryAligner6Dof.m: -------------------------------------------------------------------------------- 1 | classdef PoseTrajectoryAligner6Dof < PoseTrajectoryAlignerBase 2 | %POSETRAJECTORYALIGNER6DOF Contains functionality relating to a dense mapping system 3 | 4 | properties 5 | end 6 | 7 | methods 8 | 9 | % Constructor 10 | function obj = PoseTrajectoryAligner6Dof() 11 | % Calling the superclass constructor 12 | obj = obj@PoseTrajectoryAlignerBase(); 13 | end 14 | 15 | end 16 | 17 | methods(Static) 18 | 19 | % Calculates an alignment transform from aligned position data 20 | function T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | orientation_scaling,... 23 | sigma_init) 24 | % Checks 25 | assert(aligned_to_trajectory.length() == aligned_from_trajectory.length(), 'Vectors to be aligned must have the same size.') 26 | % The initial value of the optimization variable if not passed 27 | if nargin < 4 28 | sigma_init = zeros(6,1); 29 | end 30 | % Assigning the data matrices in the correct form 31 | X = [aligned_from_trajectory.positions aligned_from_trajectory.orientations]'; 32 | Y = [aligned_to_trajectory.positions aligned_to_trajectory.orientations]'; 33 | % Parameterizing the cost functions with dataset 34 | f_residuals = @(sigma)PoseTrajectoryAligner6Dof.get_alignment_residuals( sigma, Y, X, orientation_scaling); 35 | % Setting up the opimization options 36 | opt_TolFun = 1e-3; 37 | % opt_MaxFunEvals = 4*5000; 38 | % opt_MaxIter = 1000; 39 | opt_Display = 'final'; %'off' %'final' 40 | % options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 41 | % opt_Display, 'MaxFunEvals', opt_MaxFunEvals,... 42 | % 'MaxIter', opt_MaxIter); 43 | options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 44 | opt_Display); 45 | % Performing the optimization 46 | sigma_star = lsqnonlin(f_residuals, sigma_init, [], [], options); 47 | % Calculating the intial and final costs 48 | f_init = norm(f_residuals(sigma_init),2)^2; 49 | f_star = norm(f_residuals(sigma_star),2)^2; 50 | fprintf('Optimization initial cost: %0.2e\n', f_init); 51 | fprintf('Optimization final cost: %0.2e\n', f_star); 52 | % Extracting the individual elements of the optimization variable 53 | w_r = sigma_star(1); 54 | w_p = sigma_star(2); 55 | w_y = sigma_star(3); 56 | t_x = sigma_star(4); 57 | t_y = sigma_star(5); 58 | t_z = sigma_star(6); 59 | % The transform encoded by the optimization variables 60 | q = ypr2quat(w_y, w_p, w_r)'; 61 | t = [t_x, t_y, t_z]; 62 | T_alignment = Transformation(q, t); 63 | end 64 | 65 | % Returns the alignment residuals for a given a value for the 66 | % parameter vector (sigma) and the data vectors (X, Y) 67 | function f_sigma = get_alignment_residuals( sigma, Y, X, orientation_scaling ) 68 | % Data parameters 69 | n_points = size(X,2); 70 | % Extracting the data parts 71 | X_t = X(1:3,:); 72 | Y_t = Y(1:3,:); 73 | X_q = X(4:7,:); 74 | Y_q = Y(4:7,:); 75 | % Extracting the parameters 76 | w_r = sigma(1); 77 | w_p = sigma(2); 78 | w_y = sigma(3); 79 | t_x = sigma(4); 80 | t_y = sigma(5); 81 | t_z = sigma(6); 82 | % The transform encoded by the optimization variables 83 | q = ypr2quat(w_y, w_p, w_r)'; 84 | t = [t_x, t_y, t_z]; 85 | T_sigma = Transformation(q, t); 86 | % Constructing transform trajectories trajectory 87 | dummy_time = (1:n_points)'; 88 | X_trajectory = TransformationTrajectory(X_q', X_t', dummy_time); 89 | Y_trajectory = TransformationTrajectory(Y_q', Y_t', dummy_time); 90 | % Applying the current alignment transform 91 | X_trajectory_trans = X_trajectory.applyStaticTransformLHS(T_sigma); 92 | % Error trajectories 93 | error_trajectory_pos = Y_trajectory.positions() - X_trajectory_trans.positions(); 94 | error_trajectory_quat = Y_trajectory.getOrientationTrajectory().compose(... 95 | X_trajectory_trans.getOrientationTrajectory().inverse()); 96 | % Residuals 97 | position_error = sum(error_trajectory_pos.^2,2); 98 | orientation_error = sum(error_trajectory_quat.orientations(:,2:4).^2,2); 99 | f_sigma = position_error + (orientation_scaling * orientation_error); 100 | end 101 | end 102 | 103 | end 104 | 105 | -------------------------------------------------------------------------------- /alignment/PoseTrajectoryAlignerBase.m: -------------------------------------------------------------------------------- 1 | classdef PoseTrajectoryAlignerBase 2 | %POSETRAJECTORYALIGNER Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods 10 | 11 | % Constructor 12 | function obj = PoseTrajectoryAlignerBase() 13 | 14 | end 15 | 16 | end 17 | 18 | methods(Static, Abstract) 19 | % Calculates the alignment transform from the trajectories passed 20 | T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | orientation_scaling,... 23 | sigma_init) 24 | end 25 | 26 | methods(Static) 27 | 28 | % Truncated and resamples data streams such that they contain 29 | % measurements which correspond in time. 30 | function [fast_data_resampled, slow_data_resampled] = truncateAndResampleDatastreams(... 31 | fast_data, slow_data) 32 | % Extracting position and orientation time series 33 | % fast_positions = fast_data.positions(); 34 | % slow_positions = slow_data.positions(); 35 | % fast_orientations = fast_data.orientations(); 36 | % slow_orientations = slow_data.orientations(); 37 | % Truncating the sample times stream such that it lies inside data 38 | fast_data_start_time = fast_data.times(1); 39 | fast_data_end_time = fast_data.times(end); 40 | slow_data_start_index = find(slow_data.times() > fast_data_start_time, 1, 'first'); 41 | slow_data_end_index = find(slow_data.times() < fast_data_end_time, 1, 'last'); 42 | % Constructing time series from trancated data 43 | slow_positions_truncated_timeseries = timeseries(slow_data.positions(slow_data_start_index:slow_data_end_index,:),... 44 | slow_data.times(slow_data_start_index:slow_data_end_index)); 45 | fast_positions_truncated_timeseries = timeseries(fast_data.positions(), fast_data.times()); 46 | slow_orientations_truncated_timeseries = timeseries(slow_data.orientations(slow_data_start_index:slow_data_end_index,:),... 47 | slow_data.times(slow_data_start_index:slow_data_end_index)); 48 | fast_orientations_truncated_timeseries = timeseries(fast_data.orientations(), fast_data.times()); 49 | % Resampling the data 50 | slow_positions_resampled_timeseries = slow_positions_truncated_timeseries; 51 | fast_positions_resampled_timeseries = resample(fast_positions_truncated_timeseries,... 52 | slow_positions_truncated_timeseries.Time); 53 | slow_orientations_resampled_timeseries = slow_orientations_truncated_timeseries; 54 | fast_orientations_resampled_timeseries = PoseTrajectoryAlignerBase.quaternionResample(... 55 | fast_orientations_truncated_timeseries,... 56 | slow_orientations_truncated_timeseries.Time); 57 | % Output objects 58 | fast_data_resampled = TransformationTrajectory(fast_orientations_resampled_timeseries.Data,... 59 | fast_positions_resampled_timeseries.Data,... 60 | fast_positions_resampled_timeseries.Time); 61 | slow_data_resampled = TransformationTrajectory(slow_orientations_resampled_timeseries.Data,... 62 | slow_positions_resampled_timeseries.Data,... 63 | slow_positions_resampled_timeseries.Time); 64 | end 65 | 66 | % Resamples a quaternion time series using linear interpolation 67 | function quats_resampled = quaternionResample(quats, times) 68 | % Checks 69 | assert(times(1) > quats.Time(1), 'Sampling times need to be in between data.') 70 | assert(times(end) < quats.Time(end), 'Sampling times need to be in between data.') 71 | % Getting the interpolation amounts 72 | dummy_timeseries = timeseries((1:quats.Length)', quats.Time); 73 | interpolation_factors = dummy_timeseries.resample(times).Data; 74 | % Looping over the times and sampling 75 | slerp_quats_low = zeros(length(times),4); 76 | slerp_quats_high = zeros(length(times),4); 77 | slerp_coeffs = zeros(length(times),1); 78 | for index = 1:length(times) 79 | % Getting the interpolation end points and coeff 80 | interpolation_factor = interpolation_factors(index); 81 | low_index = floor(interpolation_factor); 82 | high_index = ceil(interpolation_factor); 83 | interpolation_coeff = interpolation_factor - low_index; 84 | % Slerping 85 | slerp_quats_low(index,:) = quats.Data(low_index,:); 86 | slerp_quats_high(index,:) = quats.Data(high_index,:); 87 | slerp_coeffs(index) = interpolation_coeff; 88 | end 89 | % Slerping 2 90 | quat_resampled_array = quatinterp(slerp_quats_low, slerp_quats_high, slerp_coeffs); 91 | % Storing as timeseries for output 92 | quats_resampled = timeseries(quat_resampled_array, times); 93 | end 94 | 95 | end 96 | 97 | end 98 | -------------------------------------------------------------------------------- /alignment/PoseTrajectoryAlignerFirstPose.m: -------------------------------------------------------------------------------- 1 | classdef PoseTrajectoryAlignerFirstPose < PoseTrajectoryAlignerBase 2 | %POSITIONTRAJECTORYALIGNERYAWONLY Contains functionality relating to a dense mapping system 3 | 4 | properties 5 | end 6 | 7 | methods 8 | 9 | % Constructor 10 | function obj = PoseTrajectoryAlignerFirstPose() 11 | % Calling the superclass constructor 12 | obj = obj@PoseTrajectoryAlignerBase(); 13 | end 14 | 15 | end 16 | 17 | methods(Static) 18 | 19 | function T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 20 | aligned_from_trajectory,... 21 | orientation_scaling,... 22 | sigma_init) 23 | % Extracting the first poses 24 | aligned_to_pose = aligned_to_trajectory.getTransformation(1); 25 | aligned_from_pose = aligned_from_trajectory.getTransformation(1); 26 | % Calculating the alignment based on this 27 | T_alignment = aligned_to_pose * aligned_from_pose.inverse(); 28 | end 29 | 30 | end 31 | 32 | end -------------------------------------------------------------------------------- /alignment/PositionTrajectoryAligner4Dof.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectoryAligner4Dof < PositionTrajectoryAlignerBase 2 | %POSITIONTRAJECTORYALIGNERYAWONLY Contains functionality relating to a dense mapping system 3 | 4 | properties 5 | end 6 | 7 | methods 8 | 9 | % Constructor 10 | function obj = PositionTrajectoryAligner4Dof() 11 | % Calling the superclass constructor 12 | obj = obj@PositionTrajectoryAlignerBase(); 13 | end 14 | 15 | end 16 | 17 | methods(Static) 18 | 19 | % Calculates an alignment transform from aligned position data 20 | function T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | sigma_init) 23 | % Checks 24 | assert(aligned_to_trajectory.length() == aligned_from_trajectory.length(), 'Vectors to be aligned must have the same size.') 25 | % The initial value of the optimization variable if not passed 26 | if nargin < 3 27 | sigma_init = zeros(4,1); 28 | end 29 | % Assigning the data matrices in the correct form 30 | X = aligned_from_trajectory.positions()'; 31 | Y = aligned_to_trajectory.positions()'; 32 | % Parameterizing the cost functions with dataset 33 | f_residuals = @(sigma)PositionTrajectoryAligner4Dof.get_alignment_residuals( sigma, Y, X ); 34 | % Setting up the opimization options 35 | opt_TolFun = 1e-3; 36 | % opt_MaxFunEvals = 4*5000; 37 | % opt_MaxIter = 1000; 38 | opt_Display = 'final'; %'off' %'final' 39 | % options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 40 | % opt_Display, 'MaxFunEvals', opt_MaxFunEvals,... 41 | % 'MaxIter', opt_MaxIter); 42 | options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 43 | opt_Display); 44 | % Performing the optimization 45 | sigma_star = lsqnonlin(f_residuals, sigma_init, [], [], options); 46 | % Calculating the intial and final costs 47 | f_init = norm(f_residuals(sigma_init),2)^2; 48 | f_star = norm(f_residuals(sigma_star),2)^2; 49 | fprintf('Optimization initial cost: %0.2e\n', f_init); 50 | fprintf('Optimization final cost: %0.2e\n', f_star); 51 | % Extracting the individual elements of the optimization variable 52 | w_z = sigma_star(1); 53 | t_x = sigma_star(2); 54 | t_y = sigma_star(3); 55 | t_z = sigma_star(4); 56 | % Reconstructing the transformation matrix 57 | R = PositionTrajectoryAligner4Dof.yprToRotationMatrix(w_z, 0, 0); 58 | t = [ t_x; t_y; t_z ]; 59 | T_alignment_matrix = [ R t ; 60 | zeros(1,3) 1 ; ]; 61 | % Transformation object construction 62 | T_alignment = Transformation(); 63 | T_alignment.initializeFromMatrix(T_alignment_matrix); 64 | end 65 | 66 | % Returns the alignment residuals for a given a value for the 67 | % parameter vector (sigma) and the data vectors (X, Y) 68 | function f_sigma = get_alignment_residuals( sigma, Y, X ) 69 | % Data parameters 70 | n_points = size(X,2); 71 | % Extracting the parameters 72 | w_z = sigma(1); 73 | t_x = sigma(2); 74 | t_y = sigma(3); 75 | t_z = sigma(4); 76 | T = repmat([t_x;t_y;t_z],1,n_points); 77 | % Forming the rotation matrix 78 | R = PositionTrajectoryAligner4Dof.yprToRotationMatrix(w_z, 0, 0); 79 | % Calculating the error 80 | f_sigma = sum((Y - R*X - T).^2,1)'; 81 | end 82 | 83 | % Converts euler angles to a rotation matrix 84 | function R = yprToRotationMatrix(w_z, w_y, w_x) 85 | % Forming the rotation matrix 86 | R = [ 1 0 0; 87 | 0 cos(w_x) sin(w_x); 88 | 0 -sin(w_x) cos(w_x);] * ... 89 | [ cos(w_y) 0 -sin(w_y); 90 | 0 1 0; 91 | sin(w_y) 0 cos(w_y);] * ... 92 | [ cos(w_z) sin(w_z) 0 ; 93 | -sin(w_z) cos(w_z) 0 ; 94 | 0 0 1 ;]; 95 | end 96 | 97 | end 98 | 99 | end -------------------------------------------------------------------------------- /alignment/PositionTrajectoryAligner6Dof.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectoryAligner6Dof < PositionTrajectoryAlignerBase 2 | %POSITIONTRAJECTORYALIGNERYAWONLY Contains functionality relating to a dense mapping system 3 | 4 | properties 5 | end 6 | 7 | methods 8 | 9 | % Constructor 10 | function obj = PositionTrajectoryAligner6Dof() 11 | % Calling the superclass constructor 12 | obj = obj@PositionTrajectoryAlignerBase(); 13 | end 14 | 15 | end 16 | 17 | methods(Static) 18 | 19 | % Calculates an alignment transform from aligned position data 20 | function T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | sigma_init) 23 | % Checks 24 | assert(aligned_to_trajectory.length() == aligned_from_trajectory.length(), 'Vectors to be aligned must have the same size.') 25 | % The initial value of the optimization variable if not passed 26 | if nargin < 3 27 | sigma_init = zeros(6,1); 28 | end 29 | % Assigning the data matrices in the correct form 30 | X = aligned_from_trajectory.positions()'; 31 | Y = aligned_to_trajectory.positions()'; 32 | % Parameterizing the cost functions with dataset 33 | f_residuals = @(sigma)PositionTrajectoryAligner6Dof.get_alignment_residuals( sigma, Y, X ); 34 | % Setting up the opimization options 35 | opt_TolFun = 1e-12; 36 | % opt_MaxFunEvals = 4*5000; 37 | % opt_MaxIter = 1000; 38 | opt_Display = 'iter'; %'off' %'final' %'iter' 39 | % options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 40 | % opt_Display, 'MaxFunEvals', opt_MaxFunEvals,... 41 | % 'MaxIter', opt_MaxIter); 42 | options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 43 | opt_Display); 44 | % Performing the optimization 45 | sigma_star = lsqnonlin(f_residuals, sigma_init, [], [], options); 46 | % Calculating the intial and final costs 47 | f_init = norm(f_residuals(sigma_init),2)^2; 48 | f_star = norm(f_residuals(sigma_star),2)^2; 49 | fprintf('Optimization initial cost: %0.2e\n', f_init); 50 | fprintf('Optimization final cost: %0.2e\n', f_star); 51 | % Extracting the individual elements of the optimization variable 52 | w_x = sigma_star(1); 53 | w_y = sigma_star(2); 54 | w_z = sigma_star(3); 55 | t_x = sigma_star(4); 56 | t_y = sigma_star(5); 57 | t_z = sigma_star(6); 58 | % Reconstructing the transformation matrix 59 | R = PositionTrajectoryAligner6Dof.yprToRotationMatrix(w_z, w_y, w_x); 60 | t = [ t_x; t_y; t_z ]; 61 | T_alignment_matrix = [ R t ; 62 | zeros(1,3) 1 ; ]; 63 | % Transformation object construction 64 | T_alignment = Transformation(); 65 | T_alignment.initializeFromMatrix(T_alignment_matrix); 66 | end 67 | 68 | % Returns the alignment residuals for a given a value for the 69 | % parameter vector (sigma) and the data vectors (X, Y) 70 | function f_sigma = get_alignment_residuals( sigma, Y, X ) 71 | % Data parameters 72 | n_points = size(X,2); 73 | % Extracting the parameters 74 | w_x = sigma(1); 75 | w_y = sigma(2); 76 | w_z = sigma(3); 77 | t_x = sigma(4); 78 | t_y = sigma(5); 79 | t_z = sigma(6); 80 | T = repmat([t_x;t_y;t_z],1,n_points); 81 | % Forming the rotation matrix 82 | R = PositionTrajectoryAligner6Dof.yprToRotationMatrix(w_z, w_y, w_x); 83 | % Calculating the error 84 | f_sigma = sum((Y - R*X - T).^2,1)'; 85 | end 86 | 87 | % Converts euler angles to a rotation matrix 88 | function R = yprToRotationMatrix(w_z, w_y, w_x) 89 | % Forming the rotation matrix 90 | R = [ 1 0 0; 91 | 0 cos(w_x) sin(w_x); 92 | 0 -sin(w_x) cos(w_x);] * ... 93 | [ cos(w_y) 0 -sin(w_y); 94 | 0 1 0; 95 | sin(w_y) 0 cos(w_y);] * ... 96 | [ cos(w_z) sin(w_z) 0 ; 97 | -sin(w_z) cos(w_z) 0 ; 98 | 0 0 1 ;]; 99 | end 100 | 101 | end 102 | 103 | end -------------------------------------------------------------------------------- /alignment/PositionTrajectoryAligner7Dof.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectoryAligner7Dof < PositionTrajectoryAlignerBase 2 | %POSITIONTRAJECTORYALIGNERYAWONLY Contains functionality relating to a dense mapping system 3 | 4 | properties 5 | end 6 | 7 | methods 8 | 9 | % Constructor 10 | function obj = PositionTrajectoryAligner7Dof() 11 | % Calling the superclass constructor 12 | obj = obj@PositionTrajectoryAlignerBase(); 13 | end 14 | 15 | end 16 | 17 | methods(Static) 18 | 19 | % Calculates an alignment transform from aligned position data 20 | function T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | sigma_init) 23 | % Checks 24 | assert(aligned_to_trajectory.length() == aligned_from_trajectory.length(), 'Vectors to be aligned must have the same size.') 25 | % The initial value of the optimization variable if not passed 26 | if nargin < 3 27 | sigma_init = [zeros(6,1) ; 1]; 28 | end 29 | % Assigning the data matrices in the correct form 30 | X = aligned_from_trajectory.positions()'; 31 | Y = aligned_to_trajectory.positions()'; 32 | % Using an external lib to do the alignment with scaling. 33 | regParams = absor(X,Y, 'doScale', 1, 'doTrans', 1); 34 | % Reconstructing the transformation matrix 35 | R = regParams.R; 36 | t = regParams.t / regParams.s; 37 | scale = regParams.s; 38 | T_alignment_matrix = [ R t ; 39 | zeros(1,3) 1/scale ; ]; 40 | % Transformation object construction 41 | T_alignment = SimTransformation(); 42 | T_alignment.initializeFromMatrix(T_alignment_matrix); 43 | end 44 | 45 | 46 | % % Parameterizing the cost functions with dataset 47 | % f_residuals = @(sigma)PositionTrajectoryAligner7Dof.get_alignment_residuals( sigma, Y, X ); 48 | % % Setting up the opimization options 49 | % opt_TolFun = 1e-12; 50 | % % opt_MaxFunEvals = 4*5000; 51 | % % opt_MaxIter = 1000; 52 | % opt_Display = 'iter'; %'off' %'final' %'iter' 53 | % % options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 54 | % % opt_Display, 'MaxFunEvals', opt_MaxFunEvals,... 55 | % % 'MaxIter', opt_MaxIter); 56 | % options = optimoptions('lsqnonlin', 'TolFun', opt_TolFun, 'Display',... 57 | % opt_Display); 58 | % % Performing the optimization 59 | % sigma_star = lsqnonlin(f_residuals, sigma_init, [], [], options); 60 | % % Calculating the intial and final costs 61 | % f_init = norm(f_residuals(sigma_init),2)^2; 62 | % f_star = norm(f_residuals(sigma_star),2)^2; 63 | % fprintf('Optimization initial cost: %0.2e\n', f_init); 64 | % fprintf('Optimization final cost: %0.2e\n', f_star); 65 | % % Extracting the individual elements of the optimization variable 66 | % w_x = sigma_star(1); 67 | % w_y = sigma_star(2); 68 | % w_z = sigma_star(3); 69 | % t_x = sigma_star(4); 70 | % t_y = sigma_star(5); 71 | % t_z = sigma_star(6); 72 | % scale = sigma_star(7); 73 | % % Reconstructing the transformation matrix 74 | % R = PositionTrajectoryAligner7Dof.yprToRotationMatrix(w_z, w_y, w_x); 75 | % t = [ t_x; t_y; t_z ]; 76 | % T_alignment_matrix = [ R t ; 77 | % zeros(1,3) 1/scale ; ]; 78 | % % Transformation object construction 79 | % T_alignment = SimTransformation(); 80 | % T_alignment.initializeFromMatrix(T_alignment_matrix); 81 | % end 82 | 83 | % % Returns the alignment residuals for a given a value for the 84 | % % parameter vector (sigma) and the data vectors (X, Y) 85 | % function f_sigma = get_alignment_residuals( sigma, Y, X ) 86 | % % Data parameters 87 | % n_points = size(X,2); 88 | % % Extracting the parameters 89 | % w_x = sigma(1); 90 | % w_y = sigma(2); 91 | % w_z = sigma(3); 92 | % t_x = sigma(4); 93 | % t_y = sigma(5); 94 | % t_z = sigma(6); 95 | % scale = sigma(7); 96 | % T = repmat([t_x;t_y;t_z],1,n_points); 97 | % % Forming the rotation matrix 98 | % R = PositionTrajectoryAligner7Dof.yprToRotationMatrix(w_z, w_y, w_x); 99 | % % Calculating the error 100 | % f_sigma = sum((Y - (R*X + T)*scale).^2,1)'; 101 | % end 102 | 103 | end 104 | 105 | end -------------------------------------------------------------------------------- /alignment/PositionTrajectoryAlignerBase.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectoryAlignerBase 2 | %POSITIONTRAJECTORYALIGNER Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods 10 | 11 | % Constructor 12 | function obj = PositionTrajectoryAlignerBase() 13 | 14 | end 15 | 16 | end 17 | 18 | methods(Static, Abstract) 19 | % Calculates the alignment transform from the trajectories passed 20 | T_alignment = calculateAlignmentTransform(aligned_to_trajectory,... 21 | aligned_from_trajectory,... 22 | sigma_init) 23 | end 24 | 25 | methods(Static) 26 | 27 | % Truncated and resamples data streams such that they contain 28 | % measurements which correspond in time. 29 | function [fast_data_resampled, slow_data_resampled] = truncateAndResampleDatastreams(... 30 | fast_data, slow_data) 31 | % Truncating the sample times stream such that it lies inside data 32 | fast_data_start_time = fast_data.times(1); 33 | fast_data_end_time = fast_data.times(end); 34 | slow_data_start_index = find(slow_data.times() > fast_data_start_time, 1, 'first'); 35 | slow_data_end_index = find(slow_data.times() < fast_data_end_time, 1, 'last'); 36 | % Constructing time series from trancated data 37 | slow_data_truncated_timeseries = timeseries(slow_data.positions(slow_data_start_index:slow_data_end_index,:),... 38 | slow_data.times(slow_data_start_index:slow_data_end_index)); 39 | fast_data_truncated_timeseries = timeseries(fast_data.positions(), fast_data.times()); 40 | % Resampling the data 41 | slow_data_resampled_timeseries = slow_data_truncated_timeseries; 42 | fast_data_resampled_timeseries = resample(fast_data_truncated_timeseries, slow_data_truncated_timeseries.Time); 43 | % Creating the trajectory objects 44 | slow_data_resampled = PositionTrajectory(slow_data_resampled_timeseries.Data, slow_data_resampled_timeseries.Time); 45 | fast_data_resampled = PositionTrajectory(fast_data_resampled_timeseries.Data, fast_data_resampled_timeseries.Time); 46 | end 47 | 48 | % TODO(alexmillane): These functions should now be included in 49 | % trajectory functionality. DELETE AFTER WAITING. 50 | % % Apply an alignment transform to a trajectory 51 | % function position_trajectory_aligned = alignTrajectory(position_trajectory, T_alignment) 52 | % % Performing alignment 53 | % trajectory_length = size(position_trajectory.Data,1); 54 | % position_trajectory_aligned_data = T_alignment * [position_trajectory.Data'; ones(1, trajectory_length)]; 55 | % position_trajectory_aligned_data = position_trajectory_aligned_data(1:3,:)'; 56 | % % Writing to timeseries 57 | % position_trajectory_aligned = timeseries(position_trajectory_aligned_data, position_trajectory.Time); 58 | % end 59 | 60 | % % Calculate the RMS of the error vector between two trajectories. 61 | % function rms_position_error = calculateRmsPositionError(... 62 | % to_trajectory, from_trajectory, T_alignment) 63 | % % Checks 64 | % if (size(to_trajectory.Data,1) ~= size(from_trajectory.Data,1)) 65 | % error('Vectors to be aligned must have the same size.') 66 | % end 67 | % % Align from_trajectory. 68 | % aligned_from_trajectory = ... 69 | % PositionTrajectoryAlignerBase.alignTrajectory(from_trajectory, T_alignment); 70 | % % Calculate the errors in x y z. 71 | % error_x_y_z = aligned_from_trajectory.Data - to_trajectory.Data; 72 | % % Calculate the residuals. 73 | % error_norm = sqrt(sum(abs(error_x_y_z).^2,2)); 74 | % rms_position_error = rms(error_norm); 75 | % end 76 | 77 | % % Calculate the trajectory length. 78 | % function trajectory_length = calculateTrajectoryLength(trajectory) 79 | % N = length(trajectory.Time); 80 | % trajectory_length = 0.0; 81 | % % Sum of all data point connections. 82 | % for i = 1:N-1 83 | % delta_s = norm(trajectory.Data(i+1,:) - trajectory.Data(i,:)); 84 | % trajectory_length = trajectory_length + delta_s; 85 | % end 86 | % end 87 | 88 | end 89 | 90 | end 91 | -------------------------------------------------------------------------------- /backend/conversions/quat2rot.m: -------------------------------------------------------------------------------- 1 | function R = quat2rot(Q) 2 | %R = quat2rot(q) 3 | % R is a 3x3 rotation matrix from quaternion q 4 | % = permute(Q,[1 3 2]); % Mike Bosse ordering. 5 | Q = permute(Q, [2 3 1]); 6 | R = [ 7 | Q(1,1,:).^2+Q(2,1,:).^2-Q(3,1,:).^2-Q(4,1,:).^2 2.0.*(Q(2,1,:).*Q(3,1,:)-Q(1,1,:).*Q(4,1,:)) 2.0.*(Q(2,1,:).*Q(4,1,:)+Q(1,1,:).*Q(3,1,:)) 8 | 2.0.*(Q(2,1,:).*Q(3,1,:)+Q(1,1,:).*Q(4,1,:)) Q(1,1,:).^2-Q(2,1,:).^2+Q(3,1,:).^2-Q(4,1,:).^2 2.0.*(Q(3,1,:).*Q(4,1,:)-Q(1,1,:).*Q(2,1,:)) 9 | 2.0.*(Q(2,1,:).*Q(4,1,:)-Q(1,1,:).*Q(3,1,:)) 2.0.*(Q(3,1,:).*Q(4,1,:)+Q(1,1,:).*Q(2,1,:)) Q(1,1,:).^2-Q(2,1,:).^2-Q(3,1,:).^2+Q(4,1,:).^2]; 10 | R = permute(R, [3,1,2]); 11 | -------------------------------------------------------------------------------- /backend/conversions/rot2quat.m: -------------------------------------------------------------------------------- 1 | function q = rot2quat(R) 2 | %q = rot2quat(R) 3 | 4 | Rv = reshape(R,9,[]); 5 | 6 | % the cubed root determinate of R is the scaling factor 7 | detR = sum(Rv([1 4 7],:).*Rv([5 8 2],:).*Rv([9 3 6],:))- ... 8 | sum(Rv([7 1 4],:).*Rv([5 8 2],:).*Rv([3 6 9],:)); 9 | Q2 = detR.^(1/3); 10 | 11 | 12 | q = sqrt(max(0,[(Q2+Rv(1,:)+Rv(5,:)+Rv(9,:)) 13 | (Q2+Rv(1,:)-Rv(5,:)-Rv(9,:)) 14 | (Q2-Rv(1,:)+Rv(5,:)-Rv(9,:)) 15 | (Q2-Rv(1,:)-Rv(5,:)+Rv(9,:))]))/2; 16 | 17 | % now copy signs 18 | g = find(Rv(6,:)=0 is an unknown global scale factor to be estimated along with R and t 30 | %and w is a user-supplied N-vector of weights. One can include/exclude any 31 | %combination of s, w, and translation t in the problem formulation. Which 32 | %parameters participate is controlled using the syntax, 33 | % 34 | % [regParams,Bfit,ErrorStats]=absor(A,B,'param1',value1,'param2',value2,...) 35 | % 36 | %with parameter/value pair options, 37 | % 38 | % 'doScale' - Boolean flag. If TRUE, the global scale factor, s, is included. 39 | % Otherwise, it is assumed that s=1. Default=FALSE. 40 | % 41 | % 'doTrans' - Boolean flag. If TRUE, the translation, t, is included. Otherwise, 42 | % zero translation is assumed. Default=TRUE. 43 | % 44 | % 'weights' - The length N-vector of weights, w. Default, no weighting. 45 | % 46 | % 47 | %OUTPUTS: 48 | % 49 | % 50 | % regParams: structure output with estimated registration parameters, 51 | % 52 | % regParams.R: The estimated rotation matrix, R 53 | % regParams.t: The estimated translation vector, t 54 | % regParams.s: The estimated scale factor. 55 | % regParams.M: Homogenous coordinate transform matrix [s*R,t;[0 0 ... 1]]. 56 | % 57 | % For 3D problems, the structure includes 58 | % 59 | % regParams.q: A unit quaternion [q0 qx qy qz] corresponding to R and 60 | % signed to satisfy max(q)=max(abs(q))>0 61 | % 62 | % For 2D problems, it includes 63 | % 64 | % regParams.theta: the counter-clockwise rotation angle about the 65 | % 2D origin 66 | % 67 | % 68 | % Bfit: The rotation, translation, and scaling (as applicable) of A that 69 | % best matches B. 70 | % 71 | % 72 | % ErrorStats: structure output with error statistics. In particular, 73 | % defining err(i)=sqrt(w(i))*norm( Bfit(:,i)-B(:,i) ), 74 | % it contains 75 | % 76 | % ErrorStats.errlsq = norm(err) 77 | % ErrorStats.errmax = max(err) 78 | % 79 | % 80 | % 81 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 82 | % Author: Matt Jacobson 83 | % Copyright, Xoran Technologies, Inc. http://www.xorantech.com 84 | 85 | 86 | %%Input option processing and set up 87 | 88 | options.doScale = 0; 89 | options.doTrans = 1; 90 | options.weights = []; 91 | 92 | for ii=1:2:length(varargin) 93 | param=varargin{ii}; 94 | val=varargin{ii+1}; 95 | if strcmpi(param,'doScale'), 96 | options.doScale=val; 97 | elseif strcmpi(param,'weights') 98 | options.weights=val; 99 | elseif strcmpi(param,'doTrans') 100 | options.doTrans=val; 101 | else 102 | error(['Option ''' param ''' not recognized']); 103 | end 104 | end 105 | 106 | doScale = options.doScale; 107 | doTrans = options.doTrans; 108 | weights = options.weights; 109 | 110 | 111 | 112 | if ~isempty(which('bsxfun')) 113 | matmvec=@(M,v) bsxfun(@minus,M,v); %matrix-minus-vector 114 | mattvec=@(M,v) bsxfun(@times,M,v); %matrix-minus-vector 115 | else 116 | matmvec=@matmvecHandle; 117 | mattvec=@mattvecHandle; 118 | end 119 | 120 | 121 | dimension=size(A,1); 122 | 123 | if dimension~=size(B,1), 124 | error 'The number of points to be registered must be the same' 125 | end 126 | 127 | 128 | %%Centering/weighting of input data 129 | 130 | if doTrans 131 | if isempty(weights) 132 | 133 | sumwts=1; 134 | 135 | lc=mean(A,2); rc=mean(B,2); %Centroids 136 | left = matmvec(A,lc); %Center coordinates at centroids 137 | right = matmvec(B,rc); 138 | 139 | else 140 | 141 | sumwts=sum(weights); 142 | 143 | weights=full(weights)/sumwts; 144 | weights=weights(:); 145 | sqrtwts=sqrt(weights.'); 146 | 147 | 148 | lc=A*weights; rc=B*weights; %weighted centroids 149 | 150 | left = matmvec(A,lc); 151 | left = mattvec(left,sqrtwts); 152 | right = matmvec(B,rc); 153 | right = mattvec(right,sqrtwts); 154 | 155 | end 156 | 157 | else 158 | 159 | if isempty(weights) 160 | 161 | sumwts=1; 162 | left=A; right=B; 163 | 164 | else 165 | 166 | sumwts=sum(weights); 167 | 168 | weights=full(weights)/sumwts; 169 | weights=weights(:); 170 | sqrtwts=sqrt(weights.'); 171 | 172 | left = mattvec(A,sqrtwts); 173 | right = mattvec(B,sqrtwts); 174 | 175 | end 176 | 177 | end 178 | 179 | M=left*right.'; 180 | 181 | 182 | %%Compute rotation matrix 183 | 184 | switch dimension 185 | 186 | case 2 187 | 188 | 189 | Nxx=M(1)+M(4); Nyx=M(3)-M(2); 190 | 191 | N=[Nxx Nyx;... 192 | Nyx -Nxx]; 193 | 194 | [V,D]=eig(N); 195 | 196 | [trash,emax]=max(real( diag(D) )); emax=emax(1); 197 | 198 | q=V(:,emax); %Gets eigenvector corresponding to maximum eigenvalue 199 | q=real(q); %Get rid of imaginary part caused by numerical error 200 | 201 | 202 | q=q*sign(q(2)+(q(2)>=0)); %Sign ambiguity 203 | q=q./norm(q); 204 | 205 | R11=q(1)^2-q(2)^2; 206 | R21=prod(q)*2; 207 | 208 | R=[R11 -R21;R21 R11]; %map to orthogonal matrix 209 | 210 | 211 | 212 | 213 | case 3 214 | 215 | [Sxx,Syx,Szx, Sxy,Syy,Szy, Sxz,Syz,Szz]=dealr(M(:)); 216 | 217 | N=[(Sxx+Syy+Szz) (Syz-Szy) (Szx-Sxz) (Sxy-Syx);... 218 | (Syz-Szy) (Sxx-Syy-Szz) (Sxy+Syx) (Szx+Sxz);... 219 | (Szx-Sxz) (Sxy+Syx) (-Sxx+Syy-Szz) (Syz+Szy);... 220 | (Sxy-Syx) (Szx+Sxz) (Syz+Szy) (-Sxx-Syy+Szz)]; 221 | 222 | [V,D]=eig(N); 223 | 224 | [trash,emax]=max(real( diag(D) )); emax=emax(1); 225 | 226 | q=V(:,emax); %Gets eigenvector corresponding to maximum eigenvalue 227 | q=real(q); %Get rid of imaginary part caused by numerical error 228 | 229 | [trash,ii]=max(abs(q)); sgn=sign(q(ii(1))); 230 | q=q*sgn; %Sign ambiguity 231 | 232 | %map to orthogonal matrix 233 | 234 | quat=q(:); 235 | nrm=norm(quat); 236 | if ~nrm 237 | disp 'Quaternion distribution is 0' 238 | end 239 | 240 | quat=quat./norm(quat); 241 | 242 | q0=quat(1); 243 | qx=quat(2); 244 | qy=quat(3); 245 | qz=quat(4); 246 | v =quat(2:4); 247 | 248 | 249 | Z=[q0 -qz qy;... 250 | qz q0 -qx;... 251 | -qy qx q0 ]; 252 | 253 | R=v*v.' + Z^2; 254 | 255 | otherwise 256 | error 'Points must be either 2D or 3D' 257 | 258 | end 259 | 260 | %% 261 | 262 | if doScale 263 | 264 | summ = @(M) sum(M(:)); 265 | 266 | sss=summ( right.*(R*left))/summ(left.^2); 267 | if doTrans 268 | t=rc-R*(lc*sss); 269 | else 270 | t=zeros(dimension,1); 271 | end 272 | 273 | else 274 | 275 | sss=1; 276 | if doTrans 277 | t=rc-R*lc; 278 | else 279 | t=zeros(dimension,1); 280 | end 281 | 282 | end 283 | 284 | 285 | regParams.R=R; 286 | regParams.t=t; 287 | regParams.s=sss; 288 | 289 | 290 | if dimension==2 291 | 292 | regParams.M=[sss*R,t;[0 0 1]]; 293 | regParams.theta=atan2(q(2),q(1))*360/pi; 294 | 295 | else%dimension=3 296 | 297 | regParams.M=[sss*R,t;[0 0 0 1]]; 298 | regParams.q=q/norm(q); 299 | 300 | end 301 | 302 | if nargout>1 303 | 304 | Bfit=(sss*R)*A; 305 | 306 | if doTrans 307 | Bfit=matmvec(Bfit,-t); 308 | end 309 | end 310 | 311 | if nargout>2 312 | 313 | l2norm = @(M,dim) sqrt(sum(M.^2,dim)); 314 | 315 | err=l2norm(Bfit-B,1); 316 | 317 | if ~isempty(weights), err=err.*sqrtwts; end 318 | 319 | ErrorStats.errlsq=norm(err)*sqrt(sumwts); %unnormalize the weights 320 | ErrorStats.errmax=max(err); 321 | 322 | 323 | end 324 | 325 | 326 | 327 | 328 | function M=matmvecHandle(M,v) 329 | %Matrix-minus-vector 330 | 331 | for ii=1:size(M,1) 332 | M(ii,:)=M(ii,:)-v(ii); 333 | end 334 | 335 | function M=mattvecHandle(M,v) 336 | %Matrix-times-vector 337 | 338 | for ii=1:size(M,1) 339 | M(ii,:)=M(ii,:).*v; 340 | end 341 | 342 | function varargout=dealr(v) 343 | 344 | varargout=num2cell(v); 345 | 346 | -------------------------------------------------------------------------------- /backend/transform_tools/k_quat_inv.m: -------------------------------------------------------------------------------- 1 | function iq = k_quat_inv(q) 2 | % Quaternions are w x y z, active. 3 | q = q'; 4 | iq = [q(1,:);-q(2:4,:)]; 5 | iq = reshape(iq,size(q)); 6 | 7 | iq = iq'; 8 | end -------------------------------------------------------------------------------- /backend/transform_tools/k_quat_mag.m: -------------------------------------------------------------------------------- 1 | function mag = k_quat_mag(q) 2 | % Quaternions are w x y z, active. 3 | % Magnitude of a quaternion. 4 | q_norm = k_quat_norm(q); 5 | mag = 2*acos(q_norm(1)); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /backend/transform_tools/k_quat_mult.m: -------------------------------------------------------------------------------- 1 | function r = k_quat_mult(p,q) 2 | % Quaternions are w x y z, active. 3 | p = p'; 4 | q = q'; 5 | 6 | M = size(p,2); 7 | N = size(q,2); 8 | if M ~= N 9 | if M==1 10 | p = repmat(p,1,N); 11 | else 12 | q = repmat(q,1,M); 13 | end 14 | end 15 | 16 | r = [ ... 17 | p(1,:).*q(1,:) - sum(p(2:4,:).*q(2:4,:)); ... 18 | p([1 1 1],:).*q(2:4,:) + ... 19 | q([1 1 1],:).*p(2:4,:) + ... 20 | p([3 4 2],:).*q([4 2 3],:) - ... 21 | p([4 2 3],:).*q([3 4 2],:) ... 22 | ]; 23 | 24 | r = r'; 25 | end 26 | -------------------------------------------------------------------------------- /backend/transform_tools/k_quat_norm.m: -------------------------------------------------------------------------------- 1 | function q = k_quat_norm(q) 2 | % qn = renorm(q) 3 | % This renormalizes each quaternion such that it has unit magnitude; 4 | 5 | q = q'; 6 | 7 | imag = 1./sqrt(sum(q.^2)); 8 | 9 | for i=1:size(q,1) 10 | q(i,:) = q(i,:).*imag(:,:); 11 | end 12 | 13 | q = q'; 14 | end 15 | 16 | 17 | -------------------------------------------------------------------------------- /backend/transform_tools/k_quat_rotate.m: -------------------------------------------------------------------------------- 1 | function rv = k_quat_rotate(q,v) 2 | %QUAT_ROTATE rotates a vector by a quaternion 3 | % Quaternions are w x y z, active. 4 | 5 | % Normal -. Mbosse 6 | q = q'; 7 | v = v'; 8 | 9 | if size(q,1) == 1 10 | rv = squeeze(quat2rot(q)) * v; 11 | else 12 | 13 | rv = zeros(3,size(v,2)); 14 | rv(1,:) = (q(1,:).^2+q(2,:).^2-q(3,:).^2-q(4,:).^2).*v(1,:)+... 15 | (2.0.*(q(2,:).*q(3,:)-q(1,:).*q(4,:))).*v(2,:)+... 16 | (2.0.*(q(2,:).*q(4,:)+q(1,:).*q(3,:))).*v(3,:); 17 | 18 | rv(2,:) = (2.0.*(q(2,:).*q(3,:)+q(1,:).*q(4,:))).*v(1,:)+... 19 | (q(1,:).^2-q(2,:).^2+q(3,:).^2-q(4,:).^2).*v(2,:)+... 20 | (2.0.*(q(3,:).*q(4,:)-q(1,:).*q(2,:))).*v(3,:); 21 | 22 | rv(3,:) = (2.0.*(q(2,:).*q(4,:)-q(1,:).*q(3,:))).*v(1,:)+... 23 | (2.0.*(q(3,:).*q(4,:)+q(1,:).*q(2,:))).*v(2,:)+... 24 | (q(1,:).^2-q(2,:).^2-q(3,:).^2+q(4,:).^2).*v(3,:); 25 | 26 | %qv = [zeros(1,size(v,2)); v]; 27 | %qrv = mul_quat(q, mul_quat(qv, diag([1 -1 -1 -1])*q)); 28 | %rv = qrv(2:4,:); 29 | end 30 | rv = rv'; 31 | end 32 | -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_identity.m: -------------------------------------------------------------------------------- 1 | function T = k_tf_identity() 2 | T = [0 0 0 1 0 0 0]; 3 | end 4 | 5 | -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_inv.m: -------------------------------------------------------------------------------- 1 | function ia = k_tf_inv(a) 2 | % Transformations are [x y z q_w q_x q_y q_z]. 3 | ia = a; %zeros(size(a)); 4 | 5 | ia(:, 4) = a(:, 4); 6 | ia(:, 5:7) = -a(:, 5:7); 7 | ia(:, 1:3) = -k_quat_rotate(ia(:, 4:7), a(:, 1:3)); 8 | end 9 | -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_mult.m: -------------------------------------------------------------------------------- 1 | function c = k_tf_mult(a, b) 2 | % Transformations are [x y z q_w q_x q_y q_z]. 3 | %compose two 6DOF configurations 4 | 5 | Na = size(a,1); 6 | Nb = size(b,1); 7 | if Na==1 && Nb>Na 8 | a = repmat(a,Nb,1); 9 | end 10 | if Nb==1 && Na>Nb 11 | b = repmat(b,Na,1); 12 | end 13 | 14 | 15 | %c = zeros(max([size(a); size(b)])); 16 | c = a; %zeros(size(a)); 17 | 18 | c(:, 1:3) = a(:, 1:3) + k_quat_rotate(a(:, 4:7), b(:, 1:3)); 19 | c(:, 4:7) = k_quat_mult(a(:, 4:7), b(:, 4:7)); 20 | end 21 | -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_pos.m: -------------------------------------------------------------------------------- 1 | function pos = k_tf_pos(T) 2 | pos = T(:, 1:3); 3 | end -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_quat.m: -------------------------------------------------------------------------------- 1 | function quat = k_tf_quat(T) 2 | quat = T(4:7); 3 | end -------------------------------------------------------------------------------- /backend/transform_tools/k_tf_transform.m: -------------------------------------------------------------------------------- 1 | function c = k_tf_transform(t, v) 2 | % Transformations are [x y z q_w q_x q_y q_z]. 3 | % Transform vector v by transform t. 4 | 5 | Nt = size(t,1); 6 | Nv = size(v,1); 7 | if Nt==1 && Nv>Nt 8 | t = repmat(t,Nv,1); 9 | end 10 | if Nv==1 && Nt>Nv 11 | v = repmat(v,Nt,1); 12 | end 13 | 14 | c = k_quat_rotate(t(:, 4:7), v) + t(:, 1:3); 15 | end 16 | -------------------------------------------------------------------------------- /data_loaders/RosDataHelper.m: -------------------------------------------------------------------------------- 1 | classdef RosDataHelper < handle 2 | %ROSDATAHELPER Helps with interpretting ros bags 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods 10 | 11 | % Constructor 12 | function obj = RosDataHelper() 13 | 14 | end 15 | 16 | end 17 | 18 | methods(Static) 19 | 20 | % Converts a set of position stamped messages to arrays 21 | function positions_stamped = convertPositionStampedMessages(position_stamped_messages) 22 | % Initializing 23 | message_num = size(position_stamped_messages,1); 24 | times = zeros(message_num,1); 25 | positions = zeros(message_num,3); 26 | % Looping over messages and extracting the data 27 | for message_index = 1:message_num 28 | message = position_stamped_messages{message_index}; 29 | times(message_index) = seconds(message.Header.Stamp); 30 | positions(message_index,:) = [message.Point.X, message.Point.Y, message.Point.Z]; 31 | end 32 | % Creating the time series for output 33 | positions_stamped.times = times; 34 | positions_stamped.positions = positions; 35 | end 36 | 37 | % TODO(millane) Convert the whole message not just part of it. 38 | % % Converts a set of position stamped messages to arrays 39 | % function [position_data] = convertPoseStampedWithCovarianceToPostionTimeseries(pose_messages) 40 | % % Initializing 41 | % message_num = size(pose_messages,1); 42 | % position_data_time = zeros(message_num,1); 43 | % position_data_data = zeros(message_num,3); 44 | % % Looping over messages and extracting the data 45 | % for message_index = 1:message_num 46 | % message = pose_messages{message_index}; 47 | % position_data_time(message_index) = seconds(message.Header.Stamp); 48 | % position_data_data(message_index,:) = [message.Pose.Pose.Position.X,... 49 | % message.Pose.Pose.Position.Y,... 50 | % message.Pose.Pose.Position.Z]; 51 | % end 52 | % % Creating the time series for output 53 | % position_data = timeseries(position_data_data, position_data_time); 54 | % end 55 | 56 | function odometry = convertOdometryMessages(odometry_messages) 57 | % Initializing 58 | message_num = size(odometry_messages,1); 59 | times = zeros(message_num,1); 60 | positions = zeros(message_num,3); 61 | orientations = zeros(message_num,4); 62 | pose_covariances = zeros(message_num,6,6); 63 | linear_velocities = zeros(message_num,3); 64 | rotational_velocities = zeros(message_num,3); 65 | velocity_covariances = zeros(message_num,6,6); 66 | % Looping over messages and extracting the data 67 | for message_index = 1:message_num 68 | message = odometry_messages{message_index}; 69 | times(message_index) = seconds(message.Header.Stamp); 70 | positions(message_index,:) = [message.Pose.Pose.Position.X,... 71 | message.Pose.Pose.Position.Y,... 72 | message.Pose.Pose.Position.Z]; 73 | orientations(message_index,:) = [message.Pose.Pose.Orientation.W,... 74 | message.Pose.Pose.Orientation.X,... 75 | message.Pose.Pose.Orientation.Y,... 76 | message.Pose.Pose.Orientation.Z]; 77 | pose_covariances(message_index,:,:) = reshape(message.Pose.Covariance, 6, 6); 78 | linear_velocities(message_index,:) = [message.Twist.Twist.Linear.X,... 79 | message.Twist.Twist.Linear.Y,... 80 | message.Twist.Twist.Linear.Z]; 81 | rotational_velocities(message_index,:) = [message.Twist.Twist.Angular.X,... 82 | message.Twist.Twist.Angular.Y,... 83 | message.Twist.Twist.Angular.Z]; 84 | velocity_covariances(message_index,:,:) = reshape(message.Twist.Covariance, 6, 6); 85 | end 86 | % Creating the time series for output 87 | odometry.times = times; 88 | odometry.positions = positions; 89 | odometry.orientation = orientations; 90 | odometry.pose_covariances = pose_covariances; 91 | odometry.linear_velocities = linear_velocities; 92 | odometry.rotational_velocities = rotational_velocities; 93 | odometry.velocity_covariances = velocity_covariances; 94 | end 95 | 96 | % Converts a set of transform stamped messages to position arrays 97 | function transforms = convertTransformStampedMessages(transform_stamped_messages) 98 | % Initializing 99 | message_num = size(transform_stamped_messages,1); 100 | times = zeros(message_num,1); 101 | positions = zeros(message_num,3); 102 | orientations = zeros(message_num,4); 103 | % Looping over messages and extracting the data 104 | for message_index = 1:message_num 105 | message = transform_stamped_messages{message_index}; 106 | times(message_index) = seconds(message.Header.Stamp); 107 | positions(message_index,:) = [ message.Transform.Translation.X,... 108 | message.Transform.Translation.Y,... 109 | message.Transform.Translation.Z]; 110 | orientations(message_index,:) = [message.Transform.Rotation.W,... 111 | message.Transform.Rotation.X,... 112 | message.Transform.Rotation.Y,... 113 | message.Transform.Rotation.Z]; 114 | end 115 | % Creating the time series for output 116 | transforms.times = times; 117 | transforms.positions = positions; 118 | transforms.orientations = orientations; 119 | end 120 | 121 | function pose_arrays = convertPoseArrayMessages(pose_array_messages) 122 | % Looping over the messages and converting 123 | message_num = size(pose_array_messages,1); 124 | pose_arrays = cell(message_num,1); 125 | for message_index = 1:message_num 126 | % Extracting this pose array 127 | pose_array = pose_array_messages{message_index}; 128 | % Extracting the data in this pose array 129 | pose_num = size(pose_array.Poses, 1); 130 | positions = zeros(pose_num,3); 131 | orientations = zeros(pose_num,4); 132 | for pose_index = 1:pose_num 133 | pose = pose_array.Poses(pose_index); 134 | positions(pose_index,:) = [ pose.Position.X,... 135 | pose.Position.Y,... 136 | pose.Position.Z]; 137 | orientations(pose_index,:) = [ pose.Orientation.W,... 138 | pose.Orientation.X,... 139 | pose.Orientation.Y,... 140 | pose.Orientation.Z]; 141 | end 142 | % Output 143 | pose_arrays{message_index}.positions = positions; 144 | pose_arrays{message_index}.orientations = orientations; 145 | end 146 | 147 | end 148 | 149 | function pose_arrays = convertTransformStampedArrayMessages(transform_stamped_array_messages) 150 | % Looping over the messages and converting 151 | message_num = size(transform_stamped_array_messages,1); 152 | pose_arrays = cell(message_num,1); 153 | for message_index = 1:message_num 154 | % Extracting this pose array 155 | transform_stamped_array = transform_stamped_array_messages{message_index}; 156 | % Extracting the data in this pose array 157 | transform_num = size(transform_stamped_array.Transforms, 1); 158 | times = zeros(transform_num,1); 159 | translations = zeros(transform_num,3); 160 | rotations = zeros(transform_num,4); 161 | for transform_index = 1:transform_num 162 | times(transform_index) = seconds(transform_stamped_array.Transforms(transform_index).Header.Stamp); 163 | transform = transform_stamped_array.Transforms(transform_index).Transform; 164 | translations(transform_index,:) = [ transform.Translation.X,... 165 | transform.Translation.Y,... 166 | transform.Translation.Z]; 167 | rotations(transform_index,:) = [ transform.Rotation.W,... 168 | transform.Rotation.X,... 169 | transform.Rotation.Y,... 170 | transform.Rotation.Z]; 171 | end 172 | % Output 173 | pose_arrays{message_index}.times = times; 174 | pose_arrays{message_index}.translations = translations; 175 | pose_arrays{message_index}.rotations = rotations; 176 | end 177 | 178 | end 179 | 180 | % Converts a range message stream to a timeseries 181 | % Converts a set of position stamped messages to arrays 182 | function ranges_stamped = convertRangeStampedMessages(range_messages) 183 | % Initializing 184 | message_num = size(range_messages,1); 185 | times = zeros(message_num,1); 186 | ranges = zeros(message_num,1); 187 | % Looping over messages and extracting the data 188 | for message_index = 1:message_num 189 | message = range_messages{message_index}; 190 | times(message_index) = seconds(message.Header.Stamp); 191 | ranges(message_index,:) = message.Range_; 192 | end 193 | % Creating the time series for output 194 | ranges_stamped.times = times; 195 | ranges_stamped.range = ranges; 196 | end 197 | 198 | % Converts a set of position stamped messages to arrays 199 | function vectors_stamped = convertVector3StampedMessages(vector3_stamped_messages) 200 | % Initializing 201 | message_num = size(vector3_stamped_messages,1); 202 | times = zeros(message_num,1); 203 | vectors = zeros(message_num,3); 204 | % Looping over messages and extracting the data 205 | for message_index = 1:message_num 206 | message = vector3_stamped_messages{message_index}; 207 | times(message_index) = seconds(message.Header.Stamp); 208 | vectors(message_index,:) = [message.Vector.X, message.Vector.Y, message.Vector.Z]; 209 | end 210 | % Creating the time series for output 211 | vectors_stamped.times = times; 212 | vectors_stamped.vectors = vectors; 213 | end 214 | 215 | 216 | end 217 | 218 | end 219 | -------------------------------------------------------------------------------- /examples/rtk_to_leica/align_rtk_to_leica.m: -------------------------------------------------------------------------------- 1 | %% align_trajectories (script) 2 | % 3 | % Aligns rtk and lieca trajectories for MBZIRC. 4 | % 5 | % -- 6 | % 7 | % (c) November 2015 by 8 | % Alexander Millane 9 | % ETH Zurich, ASL 10 | % millanea(at)ethz.ch 11 | % 12 | % Revision History: 13 | % [17.11.16, AM] file created 14 | % 15 | %% Initialization 16 | 17 | clc ; 18 | clear ; 19 | 20 | %% Loading the data 21 | 22 | % Getting the datapath (relative load) 23 | [script_path, ~, ~] = fileparts(mfilename('fullpath')); 24 | rosbag_path = strcat(script_path, '/rtk_enu_and_leica.bag'); 25 | bag_select = rosbag(rosbag_path); 26 | 27 | % Selecting the topics 28 | gps_position_select = select(bag_select, 'Topic', '/gps_position'); 29 | leica_position_select = select(bag_select, 'Topic', '/leica/position'); 30 | 31 | % Reading the messages 32 | gps_position_messages = gps_position_select.readMessages; 33 | leica_position_messages = leica_position_select.readMessages; 34 | 35 | %% Extracting the data 36 | 37 | % Creating a helper to help with data extraction 38 | ros_data_helper = RosDataHelper(); 39 | 40 | % Extracting the data to arrays 41 | gps_position = ros_data_helper.convertPositionStampedMessages(gps_position_messages); 42 | leica_position = ros_data_helper.convertPositionStampedMessages(leica_position_messages); 43 | 44 | %% Converting to trajectory objects 45 | 46 | % Shifting the times such that they start at zero 47 | start_time = gps_position.times(1); 48 | 49 | % Constructing the transformation trajectory objects 50 | gps_trajectory = PositionTrajectory(gps_position.positions,... 51 | gps_position.times - start_time); 52 | leica_trajectory = PositionTrajectory(leica_position.positions,... 53 | leica_position.times - start_time); 54 | 55 | %% Calculating the alignment transform 56 | 57 | % NOTE: Here I've decided to resample the fastest datastream, the leica 58 | % Leica - fast 59 | % GPS - slow 60 | 61 | % Creating an aligner 62 | position_trajectory_aligner = PositionTrajectoryAligner4Dof(); 63 | 64 | % Resampling the data 65 | [leica_position_resampled, gps_position_resampled] =... 66 | position_trajectory_aligner.truncateAndResampleDatastreams(leica_position,... 67 | gps_position); 68 | % Calculating the alignment transform 69 | T_alignment = position_trajectory_aligner.calculateAlignmentTransform(leica_position_resampled,... 70 | gps_position_resampled); 71 | 72 | %% Applying the alignment transform 73 | 74 | % Transforming the trajectory by the alignment transform 75 | gps_position_aligned = gps_trajectory.applyStaticTransformLHS(T_alignment); 76 | 77 | %% Plotting 78 | 79 | % 3D Positions Pre-Alignment 80 | figure() 81 | subplot(1,2,1) 82 | gps_trajectory.plot() 83 | hold on 84 | leica_trajectory.plot(); 85 | hold off 86 | axis equal 87 | grid on 88 | xlabel('x'); ylabel('y'); zlabel('z'); 89 | title('Trajectories Pre-Alignment') 90 | legend('GPS Position', 'Leica Position') 91 | % 3D Positions Post-Alignment 92 | subplot(1,2,2) 93 | gps_position_aligned.plot() 94 | hold on 95 | leica_trajectory.plot() 96 | hold off 97 | axis equal 98 | grid on 99 | xlabel('x'); ylabel('y'); zlabel('z'); 100 | title('Trajectories Post-Alignment') 101 | legend('GPS Position', 'Leica Position') 102 | -------------------------------------------------------------------------------- /examples/rtk_to_leica/rtk_enu_and_leica.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/matlab_trajectory_tools/890089031489088f62193b84d6f0f094e561d04d/examples/rtk_to_leica/rtk_enu_and_leica.bag -------------------------------------------------------------------------------- /trajectories/OrientationTrajectory.m: -------------------------------------------------------------------------------- 1 | classdef OrientationTrajectory < handle & matlab.mixin.Copyable 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % Raw Data 7 | orientations 8 | times 9 | % Properties 10 | length 11 | end 12 | 13 | methods 14 | 15 | % Constructor 16 | % Initializes the trajectory from a timeseries 17 | function obj = OrientationTrajectory(orientations, times) 18 | obj.setData(orientations, times) 19 | end 20 | 21 | % Sets the data 22 | function setData(obj, orientations, times) 23 | % Checks 24 | assert(size(orientations,1) == size(times,1), 'Must be the same number of orientations as times.'); 25 | % Storing the raw data 26 | obj.orientations = orientations; 27 | obj.times = times; 28 | % Properties 29 | obj.length = size(times, 1); 30 | end 31 | 32 | % Returns an indexed orientation quaternion 33 | function q = getOrientationQuaternion(obj, index) 34 | q = OrientationQuaternion(obj.orientations(index, :)); 35 | end 36 | 37 | % Returns the trajectory with inverted rotation matrices 38 | function inverse_trajectory = inverse(obj) 39 | inverse_trajectory = OrientationTrajectory(k_quat_inv(obj.orientations), obj.times); 40 | end 41 | 42 | % Compose this trajectory with another 43 | function transformed_trajectory = compose(obj, trajectory_other) 44 | q = k_quat_mult(obj.orientations, trajectory_other.orientations()); 45 | transformed_trajectory = OrientationTrajectory(q, obj.times); 46 | end 47 | 48 | % Transforms a trajectory of vectors 49 | function rotateTrajectory(obj, trajectory) 50 | % Checking the lengths 51 | assert(obj.length == trajectory.length()); 52 | % Getting the orientations as rotation matrices 53 | R = obj.getRotationMatrixTrajectory(); 54 | % Looping over and rotating vectory 55 | % TODO(alexmillane): Make this matrix based for speed. 56 | v_rotated = zeros(trajectory.length(), 3); 57 | for i = 1:obj.length 58 | v_rotated(i,:) = squeeze(R(i,:,:)) * trajectory.positions(i,:)'; 59 | end 60 | % Writing the data 61 | trajectory.setData(v_rotated, trajectory.times()); 62 | end 63 | 64 | % Gets a trajectory of rotation matrices 65 | function R = getRotationMatrixTrajectory(obj) 66 | % Sending the trajectory data to the generic function 67 | R = quat2rot(obj.orientations); 68 | end 69 | 70 | function magnitudes = getMagnitudes(obj) 71 | orientations_norm = k_quat_norm(obj.orientations); 72 | magnitudes = 2*acos(orientations_norm(:,1)); 73 | end 74 | 75 | end 76 | 77 | end 78 | 79 | -------------------------------------------------------------------------------- /trajectories/PositionTrajectory.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectory < handle & matlab.mixin.Copyable 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % Raw Data 7 | positions 8 | times 9 | % Properties 10 | length 11 | end 12 | 13 | methods 14 | 15 | % Constructor 16 | % Initializes the trajectory from a timeseries 17 | function obj = PositionTrajectory(positions, times) 18 | % Setting data 19 | obj.setData(positions, times) 20 | end 21 | 22 | % Sets the data 23 | function setData(obj, positions, times) 24 | % Checks 25 | assert(size(positions,1) == size(times,1), 'Times and positions must have the same length'); 26 | % Storing the data 27 | obj.positions = positions; 28 | obj.times = times; 29 | % Properties 30 | obj.length = size(times, 1); 31 | end 32 | 33 | % Gets the position at a time index 34 | function position = getPositionAtIndex(obj, index) 35 | position = obj.positions(index,:); 36 | end 37 | 38 | % Truncates the trajectory based on passed times 39 | function truncateToTimes(obj, start_time, end_time) 40 | % Finding the index bounds for truncation 41 | start_index = find(obj.times >= start_time, 1, 'first'); 42 | end_index = find(obj.times <= end_time, 1, 'last'); 43 | % Rewriting the data 44 | obj.truncateToIndicies(start_index, end_index); 45 | end 46 | 47 | % Truncates the trajectory to indicies 48 | function truncateToIndicies(obj, start_index, end_index) 49 | % Rewriting the data 50 | obj.setData(obj.positions(start_index:end_index, :),... 51 | obj.times(start_index:end_index)); 52 | end 53 | 54 | % Resamples the trajectory at the passed times 55 | function resample(obj, times) 56 | % Creating time series 57 | data_timeseries = timeseries(obj.positions, obj.times); 58 | % Resampling 59 | data_timeseries_resampled = data_timeseries.resample(times); 60 | % Updating the object 61 | obj.setData(data_timeseries_resampled.Data(), data_timeseries_resampled.Time()); 62 | end 63 | 64 | % Applies a transformation to the trajectory 65 | function transformed_trajectory = applyStaticTransformLHS(obj, T_static) 66 | % Transform vector form 67 | T_vec_static = [T_static.position T_static.orientation_quat]; 68 | % Doing transformation 69 | positions_transformed = k_tf_transform(T_vec_static, obj.positions); 70 | % Converting to object 71 | transformed_trajectory = PositionTrajectory(positions_transformed, obj.times); 72 | end 73 | 74 | % % Applies a transformation to the trajectory 75 | % % TODO 76 | % function transformed_trajectory = applyStaticTransformRHS(obj, T_static) 77 | % end 78 | 79 | % Gives the RMS error between this and a given trajectory 80 | function error = rmsErrorTo(obj, truth) 81 | % Checks 82 | assert(obj.length() == truth.length(), 'Trajectories different lengths. Consider using rmsErrorToWithResampling'); 83 | % Calculating the rms 84 | error = rms(obj.positions - truth.positions()); 85 | end 86 | 87 | % Gives the RMS error between this and a given trajectory 88 | % This function accepts trajectories of different sampling 89 | % frequencies and lengths 90 | function error = rmsErrorToWithResampling(obj, truth) 91 | % TODO(alexmillane): At the moment we assume truth is at a 92 | % higher rate. Remove this assumption 93 | % Resampling this trajectory 94 | truth_copy = truth.copy(); 95 | obj_copy = obj.copy(); 96 | obj_copy.truncateToTimes(truth_copy.times(1), truth_copy.times(end)) 97 | truth_copy.resample(obj_copy.times()); 98 | % Calculating the rms 99 | error = rms(obj_copy.positions() - truth_copy.positions()); 100 | end 101 | 102 | % Returns the error trajectory between this and a give trajectory 103 | function error_trajectory = errorTrajectoryTo(obj, truth) 104 | % Checks 105 | assert(obj.length() == truth.length(), 'Trajectories different lengths. CONSIDER WRITING FUNCTION WITH RESAMPLING'); 106 | % Calculating the error trajectory 107 | error_trajectory = sqrt(sum((obj.positions() - truth.positions()).^2, 2)); 108 | end 109 | 110 | % Returns an array of the norms of the trajectory vectors 111 | function norms = getNorms(obj) 112 | norms = zeros(obj.length(), 1); 113 | for i = 1:obj.length 114 | norms(i) = norm(obj.positions(i,:)); 115 | end 116 | end 117 | 118 | % Plots the trajectory 119 | function h = plot(obj, symbol) 120 | if nargin < 2 121 | symbol = ''; 122 | end 123 | holdstate = ishold; 124 | hold on 125 | h_temp = plot3( obj.positions(:,1),... 126 | obj.positions(:,2),... 127 | obj.positions(:,3),... 128 | symbol); 129 | plot3( obj.positions(1,1),... 130 | obj.positions(1,2),... 131 | obj.positions(1,3), 'go'); 132 | plot3( obj.positions(end,1),... 133 | obj.positions(end,2),... 134 | obj.positions(end,3), 'ro'); 135 | if nargout > 0 136 | h = h_temp; 137 | end 138 | if ~holdstate 139 | hold off 140 | end 141 | end 142 | 143 | % Plots the trajectory sample times 144 | function h = plotSampleTimes(obj, color) 145 | if nargin < 2 146 | color = 'b'; 147 | end 148 | holdstate = ishold; 149 | hold on 150 | h_temp = plot([ obj.times obj.times]',... 151 | [ zeros(obj.length(),1) ones(obj.length(),1)]',... 152 | color); 153 | h_temp = h_temp(1); 154 | if nargout > 0 155 | h = h_temp; 156 | end 157 | if ~holdstate 158 | hold off 159 | end 160 | end 161 | 162 | % Plots the trajectory sample times 163 | function h = plot3Axis(obj, symbol) 164 | if nargin < 2 165 | symbol = ''; 166 | end 167 | holdstate = ishold; 168 | hold on 169 | subplot(3,1,1) 170 | plot(obj.times, obj.positions(:,1), symbol) 171 | xlabel('Time (s)'); ylabel('X (m)'); 172 | hold on 173 | subplot(3,1,2) 174 | plot(obj.times, obj.positions(:,2), symbol) 175 | xlabel('Time (s)'); ylabel('Y (m)'); 176 | hold on 177 | subplot(3,1,3) 178 | plot(obj.times, obj.positions(:,3), symbol) 179 | xlabel('Time (s)'); ylabel('Z (m)'); 180 | 181 | if nargout > 0 182 | h = h_temp; 183 | end 184 | if ~holdstate 185 | hold off 186 | end 187 | end 188 | 189 | % Plots the trajectory in XY plane 190 | function h = plotXY(obj, symbol) 191 | if nargin < 2 192 | symbol = ''; 193 | end 194 | holdstate = ishold; 195 | hold on 196 | h_temp = plot( obj.positions(:,1),... 197 | obj.positions(:,2),... 198 | symbol); 199 | plot( obj.positions(1,1),... 200 | obj.positions(1,2), 'go'); 201 | plot( obj.positions(end,1),... 202 | obj.positions(end,2), 'ro'); 203 | if nargout > 0 204 | h = h_temp; 205 | end 206 | if ~holdstate 207 | hold off 208 | end 209 | end 210 | 211 | end 212 | 213 | end 214 | 215 | -------------------------------------------------------------------------------- /trajectories/PositionTrajectoryWithCovariance.m: -------------------------------------------------------------------------------- 1 | classdef PositionTrajectoryWithCovariance < PositionTrajectory 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | covariance 7 | end 8 | 9 | methods 10 | 11 | % Constructor 12 | % Initializes the trajectory from a timeseries 13 | function obj = PositionTrajectoryWithCovariance(positions, times, covariance) 14 | % Calling the superclass constructor 15 | obj = obj@PositionTrajectory(positions, times); 16 | % Saving the data 17 | obj.covariance = covariance; 18 | end 19 | 20 | % Returns the variance trajectory 21 | function std_trajectory = getStdTrajectory(obj) 22 | std_trajectory = [sqrt(squeeze(obj.covariance(:,1,1))),... 23 | sqrt(squeeze(obj.covariance(:,2,2))),... 24 | sqrt(squeeze(obj.covariance(:,3,3)))]; 25 | end 26 | 27 | % Plots the trajectory sample times 28 | function h = plot3AxisWithCovariance(obj, num_std, symbol) 29 | if nargin < 3 30 | symbol = ''; 31 | end 32 | holdstate = ishold; 33 | hold on 34 | 35 | % Plotting the data 36 | obj.plot3Axis(symbol); 37 | % Constructing the std lines 38 | std_trajectory = obj.getStdTrajectory(); 39 | plus_std = obj.positions + num_std * std_trajectory; 40 | minus_std = obj.positions - num_std * std_trajectory; 41 | % Potting the std lines 42 | hold on 43 | subplot(3,1,1) 44 | plot(obj.times, plus_std(:,1), '--') 45 | plot(obj.times, minus_std(:,1), '--') 46 | hold on 47 | subplot(3,1,2) 48 | plot(obj.times, plus_std(:,2), '--') 49 | plot(obj.times, minus_std(:,2), '--') 50 | hold on 51 | subplot(3,1,3) 52 | plot(obj.times, plus_std(:,3), '--') 53 | plot(obj.times, minus_std(:,3), '--') 54 | 55 | if nargout > 0 56 | h = h_temp; 57 | end 58 | if ~holdstate 59 | hold off 60 | end 61 | end 62 | 63 | end 64 | 65 | end 66 | 67 | -------------------------------------------------------------------------------- /trajectories/TrajectoryHelper.m: -------------------------------------------------------------------------------- 1 | classdef TrajectoryHelper < handle & matlab.mixin.Copyable 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | end 7 | 8 | methods 9 | 10 | % Constructor 11 | % Initializes the trajectory from a timeseries 12 | function obj = TrajectoryHelper() 13 | % Empty 14 | end 15 | 16 | end 17 | 18 | methods(Static) 19 | 20 | function truncateToMinTimes(trajectory_1, trajectory_2) 21 | % Getting the times 22 | start_time = max( trajectory_1.times(1), trajectory_2.times(1)); 23 | end_time = min( trajectory_1.times(end), trajectory_2.times(end)); 24 | % Performing truncation 25 | trajectory_1.truncateToTimes(start_time, end_time); 26 | trajectory_2.truncateToTimes(start_time, end_time); 27 | end 28 | 29 | function truncateToMinTimesAndResample(trajectory_1, trajectory_2) 30 | % Getting the times 31 | start_index = find(trajectory_1.times() > trajectory_2.times(1), 1, 'first'); 32 | end_index = find(trajectory_1.times() < trajectory_2.times(end), 1, 'last'); 33 | % Truncate to indicies 34 | trajectory_1.truncateToIndicies(start_index, end_index); 35 | % Resampling the second trajectory 36 | trajectory_2.resample(trajectory_1.times()); 37 | end 38 | 39 | end 40 | 41 | end 42 | 43 | -------------------------------------------------------------------------------- /trajectories/TransformationTrajectory.m: -------------------------------------------------------------------------------- 1 | classdef TransformationTrajectory < handle & matlab.mixin.Copyable 2 | %TRANSFORMATIONTRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % Raw data 7 | times 8 | positions 9 | orientations 10 | % Properites 11 | length 12 | end 13 | 14 | methods 15 | 16 | % Constructor 17 | % Initializes the trajectory from a timeseries 18 | function obj = TransformationTrajectory(orientations, positions, times) 19 | % Setting the data 20 | obj.setData(orientations, positions, times); 21 | end 22 | 23 | % Sets the data 24 | function setData(obj, orientations, positions, times) 25 | % Checks 26 | assert( size(times, 1) == size(orientations, 1), 'Number of orientations must be the same as the number of times') 27 | assert( size(times, 1) == size(positions, 1), 'Number of positions must be the same as the number of times') 28 | % Saving the data 29 | obj.times = times; 30 | obj.positions = positions; 31 | obj.orientations = orientations; 32 | % Properties 33 | obj.length = size(times, 1); 34 | end 35 | 36 | % Returns a transformation by index 37 | function T = getTransformation(obj, index) 38 | T = Transformation(obj.orientations(index,:), obj.positions(index,:)); 39 | end 40 | 41 | % Returns a trajectory which is the inverse of the 42 | function trajectory_inverse = inverse(obj) 43 | % Combined vector form 44 | trajectory_vec = [obj.positions obj.orientations]; 45 | % Getting inverse 46 | trajectory_inverse_vec = k_tf_inv(trajectory_vec); 47 | % Converting to object 48 | q = trajectory_inverse_vec(:,4:7); 49 | t = trajectory_inverse_vec(:,1:3); 50 | trajectory_inverse = TransformationTrajectory(q, t, obj.times); 51 | end 52 | 53 | % Truncates the trajectory to indicies 54 | function truncateToIndicies(obj, start_index, end_index) 55 | % Rewriting the data 56 | obj.setData(obj.orientations(start_index:end_index, :),... 57 | obj.positions(start_index:end_index, :),... 58 | obj.times(start_index:end_index)); 59 | end 60 | 61 | % Resamples the trajectory at the passed times 62 | function resample(obj, times) 63 | % Creating time series 64 | orientation_timeseries = timeseries(obj.orientations, obj.times); 65 | position_timeseries = timeseries(obj.positions, obj.times); 66 | % Resampling 67 | orientation_timeseries_resampled = obj.quaternionResample(orientation_timeseries, times); 68 | position_timeseries_resampled = position_timeseries.resample(times); 69 | % Updating the object 70 | obj.setData(orientation_timeseries_resampled.Data(),... 71 | position_timeseries_resampled.Data(),... 72 | position_timeseries_resampled.Time()); 73 | end 74 | 75 | % Applies a transformation to the trajectory 76 | function transformed_trajectory = applyStaticTransformLHS(obj, T_static) 77 | % Combined vector form 78 | T_vec_obj = [obj.positions obj.orientations]; 79 | T_vec_static = [T_static.position T_static.orientation_quat]; 80 | % Doing composition 81 | T_vec_transformed = k_tf_mult(T_vec_static, T_vec_obj); 82 | % Converting to object 83 | q = T_vec_transformed(:,4:7); 84 | t = T_vec_transformed(:,1:3); 85 | transformed_trajectory = TransformationTrajectory(q, t, obj.times); 86 | end 87 | 88 | % Applies a transformation to the trajectory 89 | function transformed_trajectory = applyStaticTransformRHS(obj, T_static) 90 | % Combined vector form 91 | T_vec_obj = [obj.positions obj.orientations]; 92 | T_vec_static = [T_static.position T_static.orientation_quat]; 93 | % Doing composition 94 | T_vec_transformed = k_tf_mult(T_vec_obj, T_vec_static); 95 | % Converting to object 96 | q = T_vec_transformed(:,4:7); 97 | t = T_vec_transformed(:,1:3); 98 | transformed_trajectory = TransformationTrajectory(q, t, obj.times); 99 | end 100 | 101 | % Applies a Similarity transformation to the trajectory 102 | function transformed_trajectory = applyStaticSimTransformLHS(obj, T_static) 103 | % Combined vector form 104 | T_vec_obj = [obj.positions obj.orientations]; 105 | T_vec_static = [T_static.position T_static.orientation_quat]; 106 | % Doing composition (without scale) 107 | T_vec_transformed = k_tf_mult(T_vec_static, T_vec_obj); 108 | % Converting to object (and scaling) 109 | q = T_vec_transformed(:,4:7); 110 | t = T_vec_transformed(:,1:3) * T_static.scale; 111 | transformed_trajectory = TransformationTrajectory(q, t, obj.times); 112 | end 113 | 114 | % Returns the position trajectory 115 | function position_trajectory = getPositionTrajectory(obj) 116 | position_trajectory = PositionTrajectory(obj.positions, obj.times); 117 | end 118 | 119 | function orientation_trajectory = getOrientationTrajectory(obj) 120 | orientation_trajectory = OrientationTrajectory(obj.orientations, obj.times); 121 | end 122 | 123 | % Composes this trajectory with another 124 | function transformed_trajectory = compose(obj, trajectory_other) 125 | % Combined vector form 126 | T_vec_obj = [obj.positions obj.orientations]; 127 | T_vec_other = [trajectory_other.positions trajectory_other.orientations]; 128 | % Doing composition 129 | T_vec_transformed = k_tf_mult(T_vec_obj, T_vec_other); 130 | % Converting to object 131 | q = T_vec_transformed(:,4:7); 132 | t = T_vec_transformed(:,1:3); 133 | transformed_trajectory = TransformationTrajectory(q, t, obj.times); 134 | end 135 | 136 | % Gets a windowed portion of this trajectory 137 | function windowed_trajectory = getWindowedTrajectory(obj, start_index, end_index) 138 | % Checks 139 | assert(start_index >= 1, 'Start index should be greater than 1.'); 140 | assert(end_index <= obj.length, 'End index should be less than length.'); 141 | % Creating the timeseries 142 | windowed_times = obj.times(start_index:end_index); 143 | windowed_positions = obj.positions(start_index:end_index,:); 144 | windowed_orientations = obj.orientations(start_index:end_index,:); 145 | % Creating the trajectory object 146 | windowed_trajectory = TransformationTrajectory(windowed_orientations, windowed_positions, windowed_times); 147 | end 148 | 149 | % Plots the trajectory 150 | function h = plot(obj, step, length, symbol) 151 | if nargin < 4 152 | symbol = ''; 153 | end 154 | holdstate = ishold; 155 | hold on 156 | % Plotting the position trajectory 157 | h_temp = obj.getPositionTrajectory().plot(symbol); 158 | % Plotting the transformation axis 159 | for index = 1:step:obj.length() 160 | obj.getTransformation(index).plot(length); 161 | end 162 | if nargout > 0 163 | h = h_temp; 164 | end 165 | if ~holdstate 166 | hold off 167 | end 168 | end 169 | 170 | end 171 | 172 | methods(Static) 173 | 174 | % Resamples a quaternion time series using linear interpolation 175 | function quats_resampled = quaternionResample(quats, times) 176 | % Checks 177 | assert(times(1) > quats.Time(1), 'Sampling times need to be in between data.') 178 | assert(times(end) < quats.Time(end), 'Sampling times need to be in between data.') 179 | % Getting the interpolation amounts 180 | dummy_timeseries = timeseries((1:quats.Length)', quats.Time); 181 | interpolation_factors = dummy_timeseries.resample(times).Data; 182 | % Looping over the times and sampling 183 | slerp_quats_low = zeros(length(times),4); 184 | slerp_quats_high = zeros(length(times),4); 185 | slerp_coeffs = zeros(length(times),1); 186 | for index = 1:length(times) 187 | % Getting the interpolation end points and coeff 188 | interpolation_factor = interpolation_factors(index); 189 | low_index = floor(interpolation_factor); 190 | high_index = ceil(interpolation_factor); 191 | interpolation_coeff = interpolation_factor - low_index; 192 | % Slerping 193 | slerp_quats_low(index,:) = quats.Data(low_index,:); 194 | slerp_quats_high(index,:) = quats.Data(high_index,:); 195 | slerp_coeffs(index) = interpolation_coeff; 196 | end 197 | % Slerping 2 198 | quat_resampled_array = quatinterp(slerp_quats_low, slerp_quats_high, slerp_coeffs); 199 | % Storing as timeseries for output 200 | quats_resampled = timeseries(quat_resampled_array, times); 201 | end 202 | 203 | end 204 | 205 | end 206 | 207 | -------------------------------------------------------------------------------- /types/LinearTransformationBase.m: -------------------------------------------------------------------------------- 1 | classdef LinearTransformationBase < handle & matlab.mixin.Copyable 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % The matrix form of the transformation 7 | % Dependant on the other data members and updated through 8 | % updateTransformationMatrix(obj) 9 | transformation_matrix 10 | end 11 | 12 | methods 13 | 14 | % Constructor 15 | function obj = LinearTransformationBase() 16 | % Do nothing (yet...) 17 | end 18 | 19 | % Returns the 4x4 transformation 20 | function T = getTransformationMatrix(obj) 21 | T = obj.transformation_matrix; 22 | end 23 | 24 | end 25 | 26 | % Compulsory methods for transformation types to implement 27 | methods(Abstract) 28 | 29 | % Updates the transformation matrix member from the other members 30 | updateTransformationMatrix(obj) 31 | 32 | % Initialize from a transformation matrix 33 | initializeFromMatrix(obj, T) 34 | 35 | % Returns the inverse transformation 36 | transform_inv = inverse(obj) 37 | 38 | % Transforms a set of vectors 39 | vecs_transformed = transformVectorsLHS(obj, vecs) 40 | 41 | % The times operator 42 | r = mtimes(obj_1, obj_2) 43 | 44 | end 45 | 46 | methods(Static) 47 | 48 | % Converts an array of quaternions to an array of rotation matrices 49 | function R = quat2rot(q) 50 | % Credits MBosse 51 | R = [ q(1).^2+q(2).^2-q(3).^2-q(4).^2 2.0.*(q(2).*q(3)-q(1).*q(4)) 2.0.*(q(2).*q(4)+q(1).*q(3)) 52 | 2.0.*(q(2).*q(3)+q(1).*q(4)) q(1).^2-q(2).^2+q(3).^2-q(4).^2 2.0.*(q(3).*q(4)-q(1).*q(2)) 53 | 2.0.*(q(2).*q(4)-q(1).*q(3)) 2.0.*(q(3).*q(4)+q(1).*q(2)) q(1).^2-q(2).^2-q(3).^2+q(4).^2]; 54 | end 55 | 56 | % Multiplies two quaternions 57 | function r = quatmult(p,q) 58 | % Credits MBosse 59 | r = [ p(1)*q(1) - sum(p(2:4).*q(2:4)), ... 60 | p([1 1 1]).*q(2:4) + ... 61 | q([1 1 1]).*p(2:4) + ... 62 | p([3 4 2]).*q([4 2 3]) - ... 63 | p([4 2 3]).*q([3 4 2]) ]; 64 | end 65 | 66 | % Quaternion to rotation matrix 67 | function q = rot2quat(R) 68 | % Credits MBosse 69 | Rv = reshape(R,9,1); 70 | % The cubed root determinate of R is the scaling factor 71 | detR = sum(Rv([1 4 7]).*Rv([5 8 2]).*Rv([9 3 6]))- ... 72 | sum(Rv([7 1 4]).*Rv([5 8 2]).*Rv([3 6 9])); 73 | Q2 = detR.^(1/3); 74 | % Unnormalized unsigned quaternion 75 | q = sqrt(max(0,[(Q2+Rv(1)+Rv(5)+Rv(9)) 76 | (Q2+Rv(1)-Rv(5)-Rv(9)) 77 | (Q2-Rv(1)+Rv(5)-Rv(9)) 78 | (Q2-Rv(1)-Rv(5)+Rv(9))]))/2; 79 | % Now copy signs 80 | g = find(Rv(6) 0, 'Scale must be greater than 0'); 35 | % Saving the data 36 | obj.orientation_quat = orientation_quat; 37 | obj.position = position; 38 | obj.scale = scale; 39 | % Calculating transformation matrix 40 | % NOTE(alexmillane): This might slow things down if you make 41 | % many transformation matrices 42 | obj.updateTransformationMatrix(); 43 | end 44 | 45 | % Updates the transformation matrix member from the other members 46 | function updateTransformationMatrix(obj) 47 | R = obj.quat2rot(obj.orientation_quat); 48 | p = obj.position'; 49 | s = obj.scale; 50 | obj.transformation_matrix = [R p ; 0 0 0 1/s]; 51 | end 52 | 53 | % Initialize from a transformation matrix 54 | function initializeFromMatrix(obj, T) 55 | [R, t, s] = obj.transformationMatrix2Parts(T); 56 | q = obj.rot2quat(R); 57 | obj.setData(q, t, s); 58 | end 59 | 60 | % Operator times 61 | function r = mtimes(obj_1, obj_2) 62 | % Composition through matrix multiplication 63 | T = obj_1.transformation_matrix() * obj_2.transformation_matrix(); 64 | % Extracting the parts and forming a transform object 65 | [R, t, s] = obj.transformationMatrix2Parts(T); 66 | q = obj_1.rot2quat(R); 67 | % Forming the transformation object 68 | r = SimTransformation(q, t, s); 69 | % TODO(alexmillane): Probably quicker to operate with quaternions directly 70 | % as in minkindr. 71 | % r_q = obj_1.quatmult(obj_1.orientation_quat(), obj_2.orientation_quat()) 72 | end 73 | 74 | % Transforms a set of vectors 75 | function vecs_transformed = transformVectorsLHS(obj, vecs) 76 | % % Checks 77 | % assert(size(vecs,2) == 3, 'Needs to be nx3 matrix'); 78 | % % Getting the transformed vectors 79 | % vecs_hom = [vecs'; zeros(1, size(vecs,1))]; 80 | % vecs_transformed_hom = obj.transformation_matrix * vecs_hom; 81 | % vecs_transformed = vecs_transformed_hom(1:3,:)'; 82 | end 83 | 84 | % Returns the inverse transformation 85 | function transform_inv = inverse(obj) 86 | % % TODO(alexmillane): This is an extremely inefficient way of 87 | % % doing this. Correct in the future (when needed). 88 | % T = obj.getTransformationMatrix(); 89 | % T_inv = inv(T); 90 | % R_inv = T_inv(1:3,1:3); 91 | % t_inv = T_inv(1:3,4)'; 92 | % q_inv = obj.rot2quat(R_inv); 93 | % transform_inv = Transformation(q_inv, t_inv); 94 | end 95 | 96 | % Gets the rotation matrix 97 | function R = getRotationMatrix(obj) 98 | R = obj.transformation_matrix(1:3,1:3); 99 | end 100 | 101 | % Gets the translation vector 102 | function t = getTranslationVector(obj) 103 | t = obj.transformation_matrix(1:3,4); 104 | end 105 | 106 | % Plots this transform as a set of axis 107 | function plot(obj, length) 108 | if nargin < 2 109 | length = 1; 110 | end 111 | holdstate = ishold; 112 | hold on 113 | % Generating the unit vectors for plotting 114 | unit_x = [1 0 0]'; 115 | unit_y = [0 1 0]'; 116 | unit_z = [0 0 1]'; 117 | % Rotating the axis 118 | R = obj.getRotationMatrix(); 119 | t = obj.getTranslationVector(); 120 | trans_x = R * unit_x; 121 | trans_y = R * unit_y; 122 | trans_z = R * unit_z; 123 | % Plotting 124 | quiver3(t(1), t(2), t(3), trans_x(1), trans_x(2), trans_x(3), length, 'r'); 125 | quiver3(t(1), t(2), t(3), trans_y(1), trans_y(2), trans_y(3), length, 'g'); 126 | quiver3(t(1), t(2), t(3), trans_z(1), trans_z(2), trans_z(3), length, 'b'); 127 | if ~holdstate 128 | hold off 129 | end 130 | end 131 | 132 | end 133 | 134 | methods(Static) 135 | 136 | % Returns the parts of a transformation matrix 137 | function [R, t, s] = transformationMatrix2Parts(T) 138 | R = T(1:3,1:3); 139 | t = T(1:3,4)'; 140 | s = 1/T(4,4); 141 | end 142 | 143 | end 144 | 145 | end 146 | 147 | -------------------------------------------------------------------------------- /types/Transformation.m: -------------------------------------------------------------------------------- 1 | classdef Transformation < handle & matlab.mixin.Copyable & LinearTransformationBase 2 | %TRAJECTORY Encapsulates trajectory functionality 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % Raw data 7 | orientation_quat 8 | position 9 | end 10 | 11 | methods 12 | 13 | % Constructor 14 | function obj = Transformation(orientation_quat, position) 15 | % Calling the superclass constructor 16 | obj = obj@LinearTransformationBase(); 17 | % If no arguments initialize identity transform 18 | if nargin == 0 19 | orientation_quat = [1 0 0 0]; 20 | position = [0 0 0]; 21 | end 22 | % Setting the data 23 | obj.setData(orientation_quat, position) 24 | end 25 | 26 | % Sets the data 27 | function setData(obj, orientation_quat, position) 28 | % Checks 29 | assert(length(orientation_quat) == 4, 'Quaternion incorrectly sized'); 30 | assert(length(position) == 3, 'Position incorrectly sized'); 31 | eps = 1e-6; 32 | assert(abs(norm(orientation_quat)-1) < eps, 'Quaternion does not have unit norm'); 33 | % Saving the data 34 | obj.orientation_quat = orientation_quat; 35 | obj.position = position; 36 | % Updating transformation matrix 37 | % NOTE(alexmillane): This might slow things down if you make 38 | % many transformation matrices 39 | obj.updateTransformationMatrix(); 40 | end 41 | 42 | % Updates the transformation matrix member from the other members 43 | function updateTransformationMatrix(obj) 44 | R = obj.quat2rot(obj.orientation_quat); 45 | p = obj.position'; 46 | obj.transformation_matrix = [R p ; 0 0 0 1]; 47 | end 48 | 49 | % Initialize from a transformation matrix 50 | function initializeFromMatrix(obj, T) 51 | [R, t] = obj.transformationMatrix2Parts(T); 52 | q = obj.rot2quat(R); 53 | obj.setData(q, t); 54 | end 55 | 56 | % Operator times 57 | function r = mtimes(obj_1, obj_2) 58 | % Composition through matrix multiplication 59 | T = obj_1.transformation_matrix() * obj_2.transformation_matrix(); 60 | % Extracting the parts and forming a transform object 61 | R = T(1:3,1:3); 62 | t = T(1:3,4)'; 63 | q = obj_1.rot2quat(R); 64 | % Forming the transformation object 65 | r = Transformation(q, t); 66 | % TODO(alexmillane): Probably quicker to operate with quaternions directly 67 | % as in minkindr. 68 | % r_q = obj_1.quatmult(obj_1.orientation_quat(), obj_2.orientation_quat()) 69 | end 70 | 71 | % Transforms a set of vectors 72 | function vecs_transformed = transformVectorsLHS(obj, vecs) 73 | % Checks 74 | assert(size(vecs,2) == 3, 'Needs to be nx3 matrix'); 75 | % Getting the transformed vectors 76 | vecs_hom = [vecs'; ones(1, size(vecs,1))]; 77 | vecs_transformed_hom = obj.transformation_matrix * vecs_hom; 78 | vecs_transformed = vecs_transformed_hom(1:3,:)'; 79 | end 80 | 81 | % Returns the inverse transformation 82 | function transform_inv = inverse(obj) 83 | % TODO(alexmillane): This is an extremely inefficient way of 84 | % doing this. Correct in the future (when needed). 85 | T = obj.getTransformationMatrix(); 86 | T_inv = inv(T); 87 | R_inv = T_inv(1:3,1:3); 88 | t_inv = T_inv(1:3,4)'; 89 | q_inv = obj.rot2quat(R_inv); 90 | transform_inv = Transformation(q_inv, t_inv); 91 | end 92 | 93 | % Gets the rotation matrix 94 | function R = getRotationMatrix(obj) 95 | R = obj.transformation_matrix(1:3,1:3); 96 | end 97 | 98 | % Gets the translation vector 99 | function t = getTranslationVector(obj) 100 | t = obj.transformation_matrix(1:3,4); 101 | end 102 | 103 | % Plots this transform as a set of axis 104 | function plot(obj, length) 105 | if nargin < 2 106 | length = 1; 107 | end 108 | holdstate = ishold; 109 | hold on 110 | % Generating the unit vectors for plotting 111 | unit_x = [1 0 0]'; 112 | unit_y = [0 1 0]'; 113 | unit_z = [0 0 1]'; 114 | % Rotating the axis 115 | R = obj.getRotationMatrix(); 116 | t = obj.getTranslationVector(); 117 | trans_x = R * unit_x; 118 | trans_y = R * unit_y; 119 | trans_z = R * unit_z; 120 | % Plotting 121 | quiver3(t(1), t(2), t(3), trans_x(1), trans_x(2), trans_x(3), length, 'r'); 122 | quiver3(t(1), t(2), t(3), trans_y(1), trans_y(2), trans_y(3), length, 'g'); 123 | quiver3(t(1), t(2), t(3), trans_z(1), trans_z(2), trans_z(3), length, 'b'); 124 | if ~holdstate 125 | hold off 126 | end 127 | end 128 | 129 | end 130 | 131 | methods(Static) 132 | 133 | % Returns the parts of a transformation matrix 134 | function [R, t] = transformationMatrix2Parts(T) 135 | R = T(1:3,1:3); 136 | t = T(1:3,4)'; 137 | end 138 | 139 | end 140 | 141 | end 142 | 143 | --------------------------------------------------------------------------------