├── dataset_roh ├── landmark.csv ├── noise_bearing.png ├── README.md ├── (X1.5,Y1.5).csv ├── (X3.0,Y1.5).csv ├── (X1.5,Y4.5).csv ├── (X1.5,Y3.0).csv ├── (X4.5,Y3.0).csv ├── (X4.5,Y4.5).csv ├── (X3.0,Y4.5).csv ├── (X4.5,Y1.5).csv └── (X3.0,Y3.0).csv ├── dataset_mrclam ├── mrclam1.mat ├── mrclam2.mat ├── mrclam3.mat ├── mrclam4.mat ├── mrclam5.mat ├── mrclam6.mat ├── mrclam7.mat ├── mrclam8.mat ├── mrclam9.mat ├── noise_bearing.png ├── noise_distance.png └── README.md ├── benchmark_result ├── run_eval_mrclam(2d) │ ├── ex1_position.png │ ├── ex1_orientation.png │ └── ex1_distribution.png ├── run_eval_random(map,2d) │ ├── ex1_time.png │ ├── ex2_time.png │ ├── ex1_failure.png │ ├── ex2_failure.png │ ├── ex1_position.png │ ├── ex2_position.png │ ├── ex1_06_position.png │ ├── ex1_orientation.png │ ├── ex2_orientation.png │ ├── ex1_06_distribution.png │ └── ex1_06_orientation.png ├── run_eval_random(map,3d) │ ├── ex1_time.png │ ├── ex2_time.png │ ├── ex1_failure.png │ ├── ex2_failure.png │ ├── ex1_position.png │ ├── ex2_position.png │ ├── ex1_orientation.png │ └── ex2_orientation.png ├── run_eval_random(bearing,2d) │ ├── ex1_time.png │ ├── ex2_time.png │ ├── ex1_failure.png │ ├── ex1_position.png │ ├── ex2_failure.png │ ├── ex2_position.png │ ├── ex1_orientation.png │ └── ex2_orientation.png ├── run_eval_random(distance,3d) │ ├── ex1_time.png │ ├── ex2_time.png │ ├── ex1_failure.png │ ├── ex2_failure.png │ ├── ex1_position.png │ ├── ex2_position.png │ ├── ex1_orientation.png │ └── ex2_orientation.png └── run_eval_roh(bearing,2d) │ ├── ex1_position.png │ ├── ex1_orientation.png │ ├── ex1_distribution.png │ └── ex1_reprojection.png ├── trim_rad.m ├── tran_deg2rad.m ├── tran_rad2deg.m ├── error_position.m ├── error_reprojection.m ├── test_is_true.m ├── error_orientation.m ├── apply_noise_gauss.m ├── test_is_near.m ├── tran_rad2rot.m ├── tran_rot2rad.m ├── LICENSE ├── localize2d_pose.m ├── localize2d_sayed05_toa.m ├── localize2d_sayed05_tdoa.m ├── localize3d_sayed05_toa.m ├── observe_distance_relative.m ├── localize2d_sayed05_aoa.m ├── observe_distance.m ├── localize2d_se05.m ├── observe_displacement.m ├── observe_pose.m ├── observe_bearing.m ├── localize2d_shimshoni02_algebraic.m ├── run_example.m ├── run_draw_distribution.m ├── localize2d_shimshoni02_improved.m ├── localize2d_betke97.m ├── localize3d_thomas05.m ├── run_test_localize.m ├── run_eval_roh.m ├── run_eval_mrclam.m ├── run_test_aux.m ├── README.md ├── run_conv_mrclam.m └── run_eval_random.m /dataset_roh/landmark.csv: -------------------------------------------------------------------------------- 1 | 0, 0 2 | 6, 0 3 | 6, 6 4 | 0, 6 5 | -------------------------------------------------------------------------------- /dataset_mrclam/mrclam1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam1.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam2.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam3.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam4.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam5.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam6.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam7.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam8.mat -------------------------------------------------------------------------------- /dataset_mrclam/mrclam9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/mrclam9.mat -------------------------------------------------------------------------------- /dataset_roh/noise_bearing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_roh/noise_bearing.png -------------------------------------------------------------------------------- /dataset_mrclam/noise_bearing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/noise_bearing.png -------------------------------------------------------------------------------- /dataset_mrclam/noise_distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/dataset_mrclam/noise_distance.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_mrclam(2d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_mrclam(2d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex2_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex2_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex1_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex1_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex2_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex2_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_mrclam(2d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_mrclam(2d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex2_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex2_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex1_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex1_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex2_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex2_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_mrclam(2d)/ex1_distribution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_mrclam(2d)/ex1_distribution.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex1_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex1_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex2_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex2_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex1_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex1_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex2_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex2_time.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex2_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex2_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex2_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex2_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_roh(bearing,2d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_roh(bearing,2d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex1_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex1_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex2_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex2_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex2_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex2_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex1_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex1_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex2_failure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex2_failure.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_06_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_06_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex2_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex2_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,3d)/ex2_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,3d)/ex2_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_roh(bearing,2d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_roh(bearing,2d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(bearing,2d)/ex2_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(bearing,2d)/ex2_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex1_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex1_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex2_position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex2_position.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_06_distribution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_06_distribution.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(map,2d)/ex1_06_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(map,2d)/ex1_06_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_roh(bearing,2d)/ex1_distribution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_roh(bearing,2d)/ex1_distribution.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_roh(bearing,2d)/ex1_reprojection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_roh(bearing,2d)/ex1_reprojection.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex1_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex1_orientation.png -------------------------------------------------------------------------------- /benchmark_result/run_eval_random(distance,3d)/ex2_orientation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/TriangulationToolbox/HEAD/benchmark_result/run_eval_random(distance,3d)/ex2_orientation.png -------------------------------------------------------------------------------- /trim_rad.m: -------------------------------------------------------------------------------- 1 | function [out] = trim_rad(in) 2 | %TRIM_RAD Trim the given radian angle into [-pi, +pi). 3 | % 4 | % OUT = TRIM_RAD(IN) 5 | % (matrix) IN : The given angle [rad] 6 | % (matrix) OUT: The trimmed angle [rad] 7 | % 8 | % Example: 9 | % t = trim_rad(2 * pi) 10 | % t = trim_rad(0 : pi/6 : 4*pi) 11 | 12 | out = mod(in, 2 * pi); 13 | out(out >= pi) = out(out >= pi) - 2 * pi; 14 | -------------------------------------------------------------------------------- /tran_deg2rad.m: -------------------------------------------------------------------------------- 1 | function [radian] = tran_deg2rad(degree) 2 | %TRAN_DEG2RAD Transform the given degree angle into radian angle. 3 | % 4 | % RADIAN = TRAN_DEG2RAD(DEGREE) 5 | % (matrix) DEGREE: The given angle [deg] 6 | % (matrix) RADIAN: The transformed angle [rad] 7 | % 8 | % Example: 9 | % t = tran_deg2rad(30) 10 | % t = tran_deg2rad(60) 11 | % 12 | % See also tran_rad2deg. 13 | 14 | radian = degree * pi / 180; 15 | -------------------------------------------------------------------------------- /tran_rad2deg.m: -------------------------------------------------------------------------------- 1 | function [degree] = tran_rad2deg(radian) 2 | %TRAN_RAD2DEG Transform the given radian angle into degree angle. 3 | % 4 | % DEGREE = TRAN_RAD2DEG(RADIAN) 5 | % (matrix) RADIAN: The given angle [rad] 6 | % (matrix) DEGREE: The transformed angle [deg] 7 | % 8 | % Example: 9 | % t = tran_rad2deg(pi / 6) 10 | % t = tran_rad2deg(pi / 3) 11 | % 12 | % See also tran_deg2rad. 13 | 14 | degree = radian * 180 / pi; 15 | -------------------------------------------------------------------------------- /error_position.m: -------------------------------------------------------------------------------- 1 | function [d] = error_position(p, q) 2 | %ERROR_POSITION Calculate Euclidean distance between two vectors. 3 | % 4 | % D = ERROR_POSITION(P, Q) 5 | % (matrix) P: A vector (1xN matrix, Nx1 matrix) 6 | % (matrix) Q: A vector (1xN matrix, Nx1 matrix) 7 | % (scalar) D: Euclidean distance between P and Q 8 | % 9 | % Note: Two vectors, P and Q, should be same size. 10 | % 11 | % Example: 12 | % p = [82; 3; 29]; 13 | % q = [84; 10; 18]; 14 | % d = error_position(p, q) 15 | 16 | d = norm(p - q); 17 | -------------------------------------------------------------------------------- /error_reprojection.m: -------------------------------------------------------------------------------- 1 | function [d] = error_reprojection(p, q) 2 | %ERROR_POSITION Calculate Euclidean distance between two vectors. 3 | % 4 | % D = ERROR_POSITION(P, Q) 5 | % (matrix) P: A vector (1xN matrix, Nx1 matrix) 6 | % (matrix) Q: A vector (1xN matrix, Nx1 matrix) 7 | % (scalar) D: Euclidean distance between P and Q 8 | % 9 | % Note: Two vectors, P and Q, should be same size. 10 | % 11 | % Example: 12 | % p = [82; 3; 29]; 13 | % q = [84; 10; 18]; 14 | % d = error_position(p, q) 15 | 16 | d = norm(p - q); 17 | -------------------------------------------------------------------------------- /test_is_true.m: -------------------------------------------------------------------------------- 1 | function [result] = test_is_true(value, verbose) 2 | %TEST_IS_TRUE Check whether the given value is true or not 3 | % 4 | % RESULT = TEST_IS_TRUE(VALUE, VERBOSE) 5 | % (matrix) VALUE : The given value 6 | % (scalar) VERBOSE: A flag to print its result (default: true) 7 | % (scalar) RESULT : The test result 8 | % 9 | % Example: 10 | % t = test_is_true(82 < 84, false) 11 | % t = test_is_true([10, 18] == [10, 18]) 12 | % t = test_is_true(isequal([1, 2; 3, 4], [5, 6])) 13 | % 14 | % See alo test_is_near. 15 | 16 | if nargin < 2 17 | verbose = true; 18 | end 19 | 20 | if isequal(value, ones(size(value))) 21 | result = true; 22 | if verbose, fprintf(1, 'true\n'); end 23 | else 24 | result = false; 25 | if verbose, fprintf(2, 'false\n'); end 26 | end 27 | -------------------------------------------------------------------------------- /error_orientation.m: -------------------------------------------------------------------------------- 1 | function [rad] = error_orientation(p, q) 2 | %ERROR_ORIENTATION Calculate angular difference between two sets of Euler angles. 3 | % 4 | % D = ERROR_ORIENTATION(P, Q) 5 | % (matrix) P: An Euler angle (1x3 matrix) 6 | % (matrix) Q: An Euler angle (1x3 matrix) 7 | % (scalar) D: Angular difference between two sets of Euler angles [rad] 8 | % 9 | % Note: Two vectors, P and Q, represent orientation in ZYX Euler angle. Each 10 | % element means rotation angle with repect to x, y, and z axes squentially. 11 | % 12 | % Note: Angular difference, D, is represented by a scalar value. Since heading 13 | % of an object is described by x-axis in its local coordinate, error between 14 | % two orientation comes from angular difference of two x axes. 15 | % 16 | % Example: 17 | % rad = error_orientation([0, 0, 0], [0, 0, pi]) 18 | 19 | v_p = tran_rad2rot(p) * [1; 0; 0]; 20 | v_q = tran_rad2rot(q) * [1; 0; 0]; 21 | rad = acos(v_p' * v_q); 22 | -------------------------------------------------------------------------------- /apply_noise_gauss.m: -------------------------------------------------------------------------------- 1 | function [out] = apply_noise_gauss(in, stdev) 2 | %APPLY_NOISE_GAUSS Add unbiased Gaussian noise to the given data 3 | % 4 | % OUT = APPLY_NOISE_GAUSS(IN, STDEV) 5 | % (matrix) IN : The given data (NxM matrix) 6 | % (matrix) STDEV: Standard deviation of Gaussian noise (1x1 matrix, 7 | % 1xM matrix, MxM matrix) 8 | % (matrix) OUT : The deteriorated data (NxM matrix) 9 | % 10 | % Note: The given data, IN, are composed of N vectors whose dimension is M. 11 | % 12 | % Note: 1x1 standard deviation, STDEV, for M-dimensional data is interpreted 13 | % as isotropic, STDEV * eye(M,M). Similarly, 1xM standard deviation, STDEV, 14 | % is interpreted as diagonal, diag(STDEV). 15 | % 16 | % Example: 17 | % in = zeros(100, 2); 18 | % out = apply_noise_gauss(in, 3) 19 | % out = apply_noise_gauss(in, [1, 2]) 20 | % out = apply_noise_gauss(in, [2, 1; 1, 2]) 21 | 22 | if size(stdev,1) == 1 23 | stdev = diag(stdev); 24 | end 25 | out = in + randn(size(in)) * stdev'; 26 | -------------------------------------------------------------------------------- /test_is_near.m: -------------------------------------------------------------------------------- 1 | function [result] = test_is_near(a, b, tol, verbose) 2 | %TEST_IS_TRUE Check whether two given values are near or not 3 | % 4 | % RESULT = TEST_IS_NEAR(A, B, TOL, VERBOSE) 5 | % (matrix) A : The 1st given value 6 | % (matrix) B : The 2nd given value 7 | % (scalar) TOL : Tolerance (default: 1.0e-8) 8 | % (scalar) VERBOSE: A flag to print its result (default: true) 9 | % (scalar) RESULT : The test result 10 | % 11 | % Note: Two values, A and B, should be same size. 12 | % 13 | % Example: 14 | % t = test_is_near(4.17, 4.19, 0.1) 15 | % t = test_is_near(pi, pi + eps) 16 | % t = test_is_near([3, 29], [3 + eps, 29 - eps]) 17 | % 18 | % See also test_is_true. 19 | 20 | if nargin < 3 21 | tol = 1e-8; 22 | end 23 | if nargin < 4 24 | verbose = true; 25 | end 26 | 27 | if isequal(abs(a - b) < tol, ones(size(a))) 28 | result = true; 29 | if verbose, fprintf(1, 'true\n'); end 30 | else 31 | result = false; 32 | if verbose, fprintf(2, 'false\n'); end 33 | end 34 | -------------------------------------------------------------------------------- /tran_rad2rot.m: -------------------------------------------------------------------------------- 1 | function [R] = tran_rad2rot(radian) 2 | %TRAN_RAD2ROT Compose a 3D rotation matrix from the given ZYX Euler angle. 3 | % 4 | % [R] = TRAN_RAD2ROT(RADIAN) 5 | % (matrix) RADIAN: The given Euler angle [rad] (1x3 matrix) 6 | % (matrix) R : The composed 3D rotation matrix (3x3 matrix) 7 | % 8 | % Note: Orientation, RADIAN, is represented by 1x3 matrix whose elements are 9 | % rotation angle with respect to x, y, and z axes, respectively. 10 | % 11 | % Reference: 12 | % [1] S.M. LaValle, Planning Algorithm, Cambridge, 2006, 13 | % URL: http://planning.cs.uiuc.edu/node102.html 14 | % URL: http://planning.cs.uiuc.edu/node103.html 15 | % 16 | % Example: 17 | % R = tran_rad2rot([pi/3, pi/4, pi/6]) 18 | % 19 | % See also tran_rot2rad. 20 | 21 | Rz = @(x)([cos(x), -sin(x), 0; sin(x), cos(x), 0; 0, 0, 1]); 22 | Ry = @(x)([cos(x), 0, sin(x); 0, 1, 0; -sin(x), 0, cos(x)]); 23 | Rx = @(x)([1, 0, 0; 0, cos(x), -sin(x); 0, sin(x), cos(x)]); 24 | 25 | R = Rz(radian(3)) * Ry(radian(2)) * Rx(radian(1)); 26 | -------------------------------------------------------------------------------- /tran_rot2rad.m: -------------------------------------------------------------------------------- 1 | function [radian] = tran_rot2rad(R) 2 | %TRAN_ROT2RAD Decompose the given 3D rotation matrix in ZYX Euler angle form. 3 | % 4 | % [RADIAN] = TRAN_ROT2RAD(R) 5 | % (matrix) R : The given 3D rotation matrix (3x3 matrix) 6 | % (matrix) RADIAN: The decomposed Euler angle [rad] (1x3 matrix) 7 | % 8 | % Note: Orientation, RADIAN, is represented by 1x3 matrix whose elements are 9 | % rotation angle with respect to x, y, and z axes, respectively. 10 | % 11 | % Note: R(1,1) and R(3,3) should not be zero due to singularity. 12 | % 13 | % Reference: 14 | % [1] S.M. LaValle, Planning Algorithm, Cambridge, 2006, 15 | % URL: http://planning.cs.uiuc.edu/node102.html 16 | % URL: http://planning.cs.uiuc.edu/node103.html 17 | % 18 | % Example: 19 | % R = tran_rad2rot([pi/6, pi/6, pi/6]); 20 | % angle = tran_rot2rad(R) 21 | % 22 | % See also tran_rad2rot. 23 | 24 | if det(R) < 0 25 | R = -R; 26 | end 27 | if R(1,1) == 0 || R(3,3) == 0 28 | error('An element at (1,1) or (3,3) is zero!'); 29 | end 30 | 31 | radian = [ atan2( R(3,2), R(3,3)), ... 32 | atan2(-R(3,1), norm([R(3,2), R(3,3)])), ... 33 | atan2( R(2,1), R(1,1)) ]; 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2007, Sunglok Choi 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | The views and conclusions contained in the software and documentation are those 25 | of the authors and should not be interpreted as representing official policies, 26 | either expressed or implied, of the FreeBSD Project. 27 | -------------------------------------------------------------------------------- /localize2d_pose.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_pose(data, map) 2 | %LOCALIZE2D_POSE Estimate 2D position and orientation using poses measured from landmarks (N >= 1) 3 | % 4 | % [POSE, VALID] = LOCALIZE2D_POSE(DATA, MAP) 5 | % (matrix) DATA : The measured pose from landmarks (Nx6 matrix) 6 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 7 | % (matrix) POSE : The estimated pose (1x6 matrix) 8 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 9 | % 10 | % Note: Please refer to the command, OBSERVE_POSE, for the convention of DATA, MAP, and POSE. 11 | % 12 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 13 | % element of POSE. Since this algorithm estimates 2D position and orientation, 14 | % the expected VALID is [true, true, false, false, false, true]. 15 | % 16 | % Example: 17 | % N = 1; 18 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 19 | % data = [10 * rand(N,2), zeros(N,3), 2 * pi (rand(N,1) - 0.5)]; % Random measurement 20 | % [pose, valid] = localize2d_pose(data, map) 21 | 22 | if size(data,1) < 1 23 | error('DATA has less number of observations!'); 24 | end 25 | if size(data,2) ~= 6 26 | error('DATA has wrong size!'); 27 | end 28 | 29 | theta = map(:,6) - data(:,6); 30 | c = cos(theta); 31 | s = sin(theta); 32 | x = map(:,1) - c .* data(:,1) + s .* data(:,2); 33 | y = map(:,2) - s .* data(:,1) - c .* data(:,2); 34 | 35 | pose = [mean(x), mean(y), 0, 0, 0, atan2(mean(s), mean(c))]; 36 | valid = [true, true, false, false, false, true]; 37 | -------------------------------------------------------------------------------- /dataset_roh/README.md: -------------------------------------------------------------------------------- 1 | ## Roh's Angulation Dataset 2 | 3 | _Roh's angulation dataset_ is a set of measurements acquired from a rotating IR detector and four IRED landmarks. The sensor system aims at 2D bearing-based localization, which is described in Roh's thesis [1] and paper [2] in detail. 4 | 5 | #### Landmark Map 6 | The map file, _landmark.csv_, contains position of four landmarks. 7 | * Landmark #1: (0, 0, 0) meters 8 | * Landmark #2: (6, 0, 0) meters 9 | * Landmark #3: (6, 6, 0) meters 10 | * Landmark #4: (0, 6, 0) meters 11 | 12 | #### Measurements and Their Ground Truth 13 | The data files, _(Xx.x,Yy.y).csv_, contain four bearing measurements from four landmarks at the given true position. The true position is presented in each filename, for example, measurements in _(X3.0,Y1.5).csv_ is acquired at (3.0, 1.5, 0.0) meters with orientation of (0.0, 0.0, pi/2) degrees. Each file contains 200 sets of measurements whose each column is a bearing angle (in degrees) from each landmark. 14 | * Column #1: a bearing angle (in degrees) from landmark #1 15 | * Column #2: a bearing angle (in degrees) from landmark #2 16 | * Column #3: a bearing angle (in degrees) from landmark #3 17 | * Column #4: a bearing angle (in degrees) from landmark #4 18 | 19 | #### Reference 20 | * [1] H. C. Roh, __Mobile Robot Localization based on Active Beacon System using Infrared Sensor in Indoor Environment__, KAIST, Robotics Program, Master's Thesis, 2010 [KAIST Library](http://library.kaist.ac.kr/thesis02/2010/2010M020093169_S1Ver2.pdf) 21 | * [2] H. C. Roh, Y. G. Ryu, and M. J. Chung, __Mobile Robot Localization based on Active Beacon System using Infrared Sensors in Indoor Environment__, Korea Robotics Society Annual Conference (KRoC), 2010 22 | -------------------------------------------------------------------------------- /localize2d_sayed05_toa.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_sayed05_toa(data, map) 2 | %LOCALIZE2D_SAYED05_TOA Estimate 2D position using distances measured from landmarks (N >= 3) 3 | % 4 | % [POSE, VALID] = LOCALIZE2D_SAYED05_TOA(DATA, MAP) 5 | % (matrix) DATA : The measured distances from landmarks (Nx1 matrix) 6 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 7 | % (matrix) POSE : The estimated pose (1x6 matrix) 8 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 9 | % 10 | % Note: Please refer to the command, OBSERVE_DISTANCE, for the convention of DATA, 11 | % MAP, and POSE. 12 | % 13 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 14 | % element of POSE. Since this algorithm estimates 2D position, the expected 15 | % VALID is [true, true, false, false, false, false]. 16 | % 17 | % Reference: 18 | % [1] A. H. Sayed et al., Network-based Wireless Location, 19 | % IEEE Signal Processing Magazine, Vol. 24, No. 4, 2005 20 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1458275 21 | % 22 | % Example: 23 | % N = 3; 24 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 25 | % data = 10 * rand(N,1); % Random measurement 26 | % [pose, valid] = localize2d_sayed05_toa(data, map) 27 | % 28 | % See also localize_sayed05_tdoa, localize3d_sayed05_toa. 29 | 30 | if size(data,1) < 3 31 | error('DATA has less number of observations!'); 32 | end 33 | if size(data,2) ~= 1 34 | error('DATA has wrong size!'); 35 | end 36 | 37 | origin = map(1,1:2); 38 | H = map(2:end,1:2) - repmat(origin, size(map,1) - 1, 1); 39 | if rank(H) < 2 40 | warning('All landmarks on MAP lie on a line!'); 41 | end 42 | b = (H(:,1).^2 + H(:,2).^2 - data(2:end).^2 + data(1)^2) * 0.5; 43 | x = pinv(H) * b; % Solve H * x = b 44 | 45 | pose = [x' + origin, 0, 0, 0, 0]; 46 | valid = [true, true, false, false, false, false]; 47 | -------------------------------------------------------------------------------- /localize2d_sayed05_tdoa.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_sayed05_tdoa(data, map) 2 | %LOCALIZE2D_SAYED05_TDOA Estimate 2D position using relative distances measured from landmarks (N >= 3) 3 | % 4 | % [POSE, VALID] = LOCALIZE2D_SAYED05_TDOA(DATA, MAP) 5 | % (matrix) DATA : The relative distances w.r.t. the first observation (Nx1 matrix) 6 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 7 | % (matrix) POSE : The estimated pose (1x6 matrix) 8 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 9 | % 10 | % Note: Please refer to the command, OBSERVE_DISTANCE, for the convention of DATA, 11 | % MAP, and POSE. 12 | % 13 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 14 | % element of POSE. Since this algorithm estimates 2D position, the expected 15 | % VALID is [true, true, false, false, false, false]. 16 | % 17 | % Reference: 18 | % [1] A. H. Sayed et al., Network-based Wireless Location, 19 | % IEEE Signal Processing Magazine, Vol. 24, No. 4, 2005 20 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1458275 21 | % 22 | % Example: 23 | % N = 3; 24 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 25 | % data = 10 * rand(N,1); % Random measurement 26 | % [pose, valid] = localize2d_sayed05_tdoa(data, map) 27 | % 28 | % See also localize_sayed05_toa. 29 | 30 | if size(data,1) < 3 31 | error('DATA has less number of observations!'); 32 | end 33 | if size(data,2) ~= 1 34 | error('DATA has wrong size!'); 35 | end 36 | 37 | origin = map(1,1:2); 38 | H = map(2:end,1:2) - repmat(origin, size(map,1) - 1, 1); 39 | if rank(H) < 2 40 | warning('All landmarks on MAP lie on a line!'); 41 | end 42 | c = -data(2:end); 43 | d = 0.5 * (H(:,1).^2 + H(:,2).^2 - data(2:end).^2); 44 | x = pinv(H) * (data(1) * c + d); % Solve H * x = data(1) * c + d 45 | 46 | pose = [x' + origin, 0, 0, 0, 0]; 47 | valid = [true, true, false, false, false, false]; 48 | -------------------------------------------------------------------------------- /localize3d_sayed05_toa.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize3d_sayed05_toa(data, map) 2 | %LOCALIZE3D_SAYED05_TOA Estimate 3D position using distances measured from landmarks (N >= 4) 3 | % 4 | % [POSE, VALID] = LOCALIZE3D_SAYED05_TOA(DATA, MAP) 5 | % (matrix) DATA : The measured distances from landmarks (Nx1 matrix) 6 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 7 | % (matrix) POSE : The estimated pose (1x6 matrix) 8 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 9 | % 10 | % Note: Please refer to the command, OBSERVE_DISTANCE, for the convention of DATA, 11 | % MAP, and POSE. 12 | % 13 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 14 | % element of POSE. Since this algorithm estimates 3D position, the expected 15 | % VALID is [true, true, true, false, false, false]. 16 | % 17 | % Reference: 18 | % [1] A. H. Sayed et al., Network-based Wireless Location, 19 | % IEEE Signal Processing Magazine, Vol. 24, No. 4, 2005 20 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1458275 21 | % 22 | % Example: 23 | % N = 4; 24 | % map = [10 * rand(N,3), zeros(N,3)]; % Random 2D landmark map 25 | % data = 10 * rand(N,1); % Random measurement 26 | % [pose, valid] = localize3d_sayed05_toa(data, map) 27 | % 28 | % See also localize2d_sayed05_toa, localize3d_thomas05. 29 | 30 | if size(data,1) < 4 31 | error('DATA has less number of observations!'); 32 | end 33 | if size(data,2) ~= 1 34 | error('DATA has wrong size!'); 35 | end 36 | 37 | origin = map(1,1:3); 38 | H = map(2:end,1:3) - repmat(origin, size(map,1) - 1, 1); 39 | dof = rank(H); 40 | if dof < 2 41 | warning('All landmarks on MAP lie on a same line!'); 42 | elseif dof < 3 43 | warning('All landmarks in MAP lie on a same plane!'); 44 | end 45 | b = (H(:,1).^2 + H(:,2).^2 + H(:,3).^2 - data(2:end).^2 + data(1)^2) * 0.5; 46 | x = pinv(H) * b; % Solve H * x = b 47 | 48 | pose = [x' + origin, 0, 0, 0]; 49 | valid = [true, true, true, false, false, false]; 50 | -------------------------------------------------------------------------------- /observe_distance_relative.m: -------------------------------------------------------------------------------- 1 | function [obsData, obsMap] = observe_distance_relative(map, pose, visibleRate) 2 | %OBSERVE_DISTANCE_RELATIVE Measure distance of landmarks relative to the first observation. 3 | % 4 | % [OBS_DATA, OBS_MAP] = CALCULATE_DISTANCE_RELATIVE(MAP, POSE, VISIBLE_RATE) 5 | % (matrix) MAP : A landmark map (Nx6 matrix) 6 | % (matrix) POSE : Pose of the target object (1x6 matrix) 7 | % (scalar) VISIBLE_RATE: Visible probability of landmarks (default: 1) 8 | % (matrix) OBS_DATA : The relative distance from POSE to landmarks (Mx1 matrix) 9 | % (matrix) OBS_MAP : The landmark map of measured landmarks (Mx6 matrix) 10 | % 11 | % Note: Pose of an object, POSE, is represented by 1x6 vector whose first three 12 | % columns represent position of the object, (x, y, z), and last three 13 | % columns represent orientation of the object, (r_x, r_y, r_z) [rad]. 14 | % 15 | % Note: A landmark map, MAP, is Nx6 matrix which contains position and 16 | % orientation of landmarks in the world coordinate. Its first three columns 17 | % represents position of landmarks, (x, y, z). Its last three columns represent 18 | % orientation of landmarks, (r_x, r_y, r_z) [rad]. 19 | % 20 | % Note: The number of output data, M, will be approximately VISIBLE_RATE * N. 21 | % If there is no visible landmark, OBS_DATA and OBS_MAP will be an empty matrix. 22 | % Please use the command, ISEMPTY, to identify an empty matrix. 23 | % 24 | % Example: 25 | % map = [ 0, 0, 5, 0, 0, 0; ... 26 | % 5, 0, 5, 0, 0, 0; ... 27 | % 5, 5, 5, 0, 0, 0; ... 28 | % 0, 5, 5, 0, 0, 0 ]; 29 | % pose = [ 3, 2, 9, 0, 0, pi / 2 ]; 30 | % [obsData, obsMap] = observe_distance_relative(map, pose) 31 | % 32 | % See also observe_distance, observe_bearing, observe_displacement, observe_pose. 33 | 34 | if nargin < 3 35 | visibleRate = 1; 36 | end 37 | 38 | [obsData, obsMap] = observe_distance(map, pose, visibleRate); 39 | if ~isempty(obsData) 40 | obsData(2:end) = obsData(2:end) - obsData(1); 41 | end 42 | -------------------------------------------------------------------------------- /localize2d_sayed05_aoa.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_sayed05_aoa(data, map) 2 | %LOCALIZE2D_SAYED05_AOA Estimate 2D position and orientation using displacements 3 | % measured from landmarks (N >= 2) 4 | % 5 | % [POSE, VALID] = LOCALIZE2D_SAYED05_AOA(DATA, MAP) 6 | % (matrix) DATA : The measured displacement from landmarks (Nx3 matrix) 7 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 8 | % (matrix) POSE : The estimated pose (1x6 matrix) 9 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 10 | % 11 | % Note: Please refer to the command, OBSERVE_DISPLACEMENT, for the convention of DATA, 12 | % MAP, and POSE. 13 | % 14 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 15 | % element of POSE. Since this algorithm estimates 2D position and orientation, 16 | % the expected VALID is [true, true, false, false, false, true]. 17 | % 18 | % Note: This implementation is extended from Sayed et al. [1], so it can utilize 19 | % angle-of-arrival (AOA) obtained at a mobile station (MS), not base station (BS). 20 | % 21 | % Reference: 22 | % [1] A. H. Sayed et al., Network-based Wireless Location, 23 | % IEEE Signal Processing Magazine, Vol. 24, No. 4, 2005 24 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1458275 25 | % 26 | % Example: 27 | % N = 2; 28 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 29 | % data = [10 * rand(N,2), zeros(N,1)]; % Random measurement 30 | % [pose, valid] = localize2d_sayed05_aoa(data, map) 31 | % 32 | % See also localize2d_se05 33 | 34 | if size(data,1) < 2 35 | error('DATA has less number of observations!'); 36 | end 37 | if size(data,2) ~= 3 38 | error('DATA has wrong size!'); 39 | end 40 | 41 | n = size(data,1); 42 | A4 = zeros(2 * n,1); 43 | A4(1:2:end) = -data(:,2); 44 | A4(2:2:end) = +data(:,1); 45 | A = [repmat(eye(2,2), n, 1), reshape(data(:,1:2)', 2 * n, []), A4]; 46 | b = reshape(map(:,1:2)', 2 * n, []); 47 | x = pinv(A) * b; 48 | x = x / norm(x(3:4)); 49 | 50 | pose = [x(1), x(2), 0, 0, 0, atan2(x(4), x(3))]; 51 | valid = [true, true, false, false, false, true]; 52 | -------------------------------------------------------------------------------- /observe_distance.m: -------------------------------------------------------------------------------- 1 | function [obsData, obsMap] = observe_distance(map, pose, visibleRate) 2 | %OBSERVE_DISTANCE Measure distance from the given pose to landmarks. 3 | % 4 | % [OBS_DATA, OBS_MAP] = CALCULATE_DISTANCE(MAP, POSE, VISIBLE_RATE) 5 | % (matrix) MAP : A landmark map (Nx6 matrix) 6 | % (matrix) POSE : Pose of the target object (1x6 matrix) 7 | % (scalar) VISIBLE_RATE: Visible probability of landmarks (default: 1) 8 | % (matrix) OBS_DATA : The measured distance from POSE to landmarks (Mx1 matrix) 9 | % (matrix) OBS_MAP : The landmark map of measured landmarks (Mx6 matrix) 10 | % 11 | % Note: Pose of an object, POSE, is represented by 1x6 vector whose first three 12 | % columns represent position of the object, (x, y, z), and last three 13 | % columns represent orientation of the object, (r_x, r_y, r_z) [rad]. 14 | % 15 | % Note: A landmark map, MAP, is Nx6 matrix which contains position and 16 | % orientation of landmarks in the world coordinate. Its first three columns 17 | % represents position of landmarks, (x, y, z). Its last three columns represent 18 | % orientation of landmarks, (r_x, r_y, r_z) [rad]. 19 | % 20 | % Note: The number of output data, M, will be approximately VISIBLE_RATE * N. 21 | % If there is no visible landmark, OBS_DATA and OBS_MAP will be an empty matrix. 22 | % Please use the command, ISEMPTY, to identify an empty matrix. 23 | % 24 | % Example: 25 | % map = [ 0, 0, 5, 0, 0, 0; ... 26 | % 5, 0, 5, 0, 0, 0; ... 27 | % 5, 5, 5, 0, 0, 0; ... 28 | % 0, 5, 5, 0, 0, 0 ]; 29 | % pose = [ 3, 2, 9, 0, 0, pi / 2 ]; 30 | % [obsData, obsMap] = observe_distance(map, pose) 31 | % 32 | % See also observe_distance_relative, observe_bearing, observe_displacement, observe_pose. 33 | 34 | if nargin < 3 35 | visibleRate = 1; 36 | end 37 | 38 | isVisible = rand(size(map,1), 1) < visibleRate; % Select visible landmarks 39 | obsMap = map(isVisible,:); 40 | obsNum = size(obsMap,1); 41 | obsDim = 1; 42 | 43 | obsData = zeros(obsNum,obsDim); 44 | if obsNum > 0 45 | delta = obsMap(:,1:3) - repmat(pose(1:3), obsNum, 1); 46 | obsData = sqrt(delta(:,1).^2 + delta(:,2).^2 + delta(:,3).^2); % Calculate distance 47 | end 48 | -------------------------------------------------------------------------------- /localize2d_se05.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_se05(data, map) 2 | %LOCALIZE2D_SE05 Estimate 2D position and orientation using displacements 3 | % measured from landmarks (N >= 2) 4 | % 5 | % [POSE, VALID] = LOCALIZE2D_SE05(DATA, MAP) 6 | % (matrix) DATA : The measured displacement from landmarks (Nx3 matrix) 7 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 8 | % (matrix) POSE : The estimated pose (1x6 matrix) 9 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 10 | % 11 | % Note: Please refer to the command, OBSERVE_DISPLACEMENT, for the convention of DATA, 12 | % MAP, and POSE. 13 | % 14 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 15 | % element of POSE. Since this algorithm estimates 2D position and orientation, 16 | % the expected VALID is [true, true, false, false, false, true]. 17 | % 18 | % Note: This implementation is extended from Se et al. [1], so it can take into account 19 | % more than two measurements in lease-squares sense. 20 | % 21 | % Reference: 22 | % [1] S. Se et al., Vision-Based Global Localization and Mapping for Mobile Robots, 23 | % IEEE Transactions on Robotics, Vol. 21, No. 3, 2005 24 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1435480 25 | % 26 | % Example: 27 | % N = 2; 28 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 29 | % data = [10 * rand(N,2), zeros(N,1)]; % Random measurement 30 | % [pose, valid] = localize2d_se05(data, map) 31 | % 32 | % See also localize2d_sayed05_toa. 33 | 34 | if size(data,1) < 2 35 | error('DATA has less number of observations!'); 36 | end 37 | if size(data,2) ~= 3 38 | error('DATA has wrong size!'); 39 | end 40 | 41 | A = data(2:end,1) - data(1,1); 42 | B = data(2:end,2) - data(1,2); 43 | C = map(2:end,1) - map(1,1); 44 | D = map(2:end,2) - map(1,2); 45 | theta = pinv([A, B; B, -A]) * [C; D]; % Solve 'theta' in least squares 46 | align = map(1,1:2)' - [data(1,1), data(1,2); data(1,2), -data(1,1)] * theta; 47 | 48 | pose = [align(1), align(2), 0, 0, 0, -atan2(theta(2), theta(1))]; 49 | % Q) Why '-atan2(...)' for orientation? 50 | % A) In this toolbox, CCW is positive rotation, but the paper [1] uses it as negative. 51 | valid = [true, true, false, false, false, true]; 52 | -------------------------------------------------------------------------------- /observe_displacement.m: -------------------------------------------------------------------------------- 1 | function [obsData, obsMap] = observe_displacement(map, pose, visibleRate) 2 | %OBSERVE_DISPLACEMENT Measure displacement from the given pose to landmarks. 3 | % 4 | % [OBS_DATA, OBS_MAP] = OBSERVE_DISPLACEMENT(MAP, POSE, VISIBLE_RATE) 5 | % (matrix) MAP : A landmark map (Nx6 matrix) 6 | % (matrix) POSE : Pose of the target object (1x6 matrix) 7 | % (scalar) VISIBLE_RATE: Visible probability of landmarks (default: 1) 8 | % (matrix) OBS_DATA : The measured displacement from POSE to landmarks (Mx3 matrix) 9 | % (matrix) OBS_MAP : The landmark map of measured landmarks (Mx6 matrix) 10 | % 11 | % Note: Pose of an object, POSE, is represented by 1x6 vector whose first three 12 | % columns represent position of the object, (x, y, z), and last three 13 | % columns represent orientation of the object, (r_x, r_y, r_z) [rad]. 14 | % 15 | % Note: A landmark map, MAP, is Nx6 matrix which contains position and 16 | % orientation of landmarks in the world coordinate. Its first three columns 17 | % represents position of landmarks, (x, y, z). Its last three columns represent 18 | % orientation of landmarks, (r_x, r_y, r_z) [rad]. 19 | % 20 | % Note: The number of output data, M, will be approximately VISIBLE_RATE * N. 21 | % If there is no visible landmark, OBS_DATA and OBS_MAP will be an empty matrix. 22 | % Please use the command, ISEMPTY, to identify an empty matrix. 23 | % 24 | % Note: The measured displacement, OBS_DATA, is represented by Mx3 matrix whose i-th row 25 | % is relative position of i-th landmark in OBS_MAP with respect to the given pose, POSE. 26 | % 27 | % Example: 28 | % map = [ 0, 0, 5, 0, 0, 0; ... 29 | % 5, 0, 5, 0, 0, 0; ... 30 | % 5, 5, 5, 0, 0, 0; ... 31 | % 0, 5, 5, 0, 0, 0 ]; 32 | % pose = [ 3, 2, 9, 0, 0, pi / 2 ]; 33 | % [obsData, obsMap] = observe_displacement(map, pose) 34 | % 35 | % See also observe_distance, observe_distance_relative, observe_bearing, observe_pose. 36 | 37 | if nargin < 3 38 | visibleRate = 1; 39 | end 40 | 41 | isVisible = rand(size(map,1), 1) < visibleRate; % Select visible landmarks 42 | obsMap = map(isVisible,:); 43 | obsNum = size(obsMap,1); 44 | obsDim = 3; 45 | 46 | obsData = zeros(obsNum,obsDim); 47 | if obsNum > 0 48 | delta = obsMap(:,1:3) - repmat(pose(1:3), obsNum, 1); 49 | obsData = delta * tran_rad2rot(pose(4:6)); % Calculate displacement 50 | % a = R' * b --> a' = b' * R 51 | end 52 | -------------------------------------------------------------------------------- /dataset_mrclam/README.md: -------------------------------------------------------------------------------- 1 | ## UTIAS MRCLAM Dataset 2 | UTIAS Multi-Robot Cooperative Localization and Mapping Dataset (a.k.a. MRCLAM dataset) consists of 9 sets of data including odometry, distance and bearing measurements of 15 landmarks from 5 robots. The dataset originally aims cooperative localization and SLAM, but we utilize it for landmark-based localization from instantaneously available observation. 3 | 4 | We convert MRCLAM dataset to easily apply to landmark-based localization. The instantaneously available data are extracted as observed measurements during very small motion (0.01 meters and 0.01 radians) and short time (at most 1 second). We reject the observation whose number of measurements is less than 3. Outlier measurements are also removed during conversion. Its conversion for each set is presented in [run\_conv\_mrclam.m](https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_conv_mrclam.m). The converted sets of data are written in MATLAB MAT files such as `mrclam1.mat`. 5 | 6 | You can get more detail on MRCLAM dataset from its website and paper [1]. Please cite their paper if you use this dataset. 7 | 8 | #### Landmark Map, Ground Truth, and Measurements 9 | The data files, `mrclam?.mat`, contains three variables: _landmark_, _groundtruth_, and _measurement_. The variable _landmark_ contains 15 lines of information with 3 columns which sequentially mean landmark ID and 2D position. In contrast to the original dataset, the landmark IDs are rearranged to begin with 1. The variable _groundtruth_ contains 4 columns of data which are experiment ID and true 2D pose (position and orientation) of the robot. The experiment IDs start from 1 and its number is same with the number of lines. The variable _measurement_ includes 4 columns of data which represent the experiment ID, observed landmark ID, and its range and bearing measurements. Observations with same experiment ID mean a single set of experiments. Units of all values are meter for distance and radian for angle. In overall, three variables contains the following. 10 | * _landmark_: landmark ID, position X, position Y (of each landmark) 11 | * _groundtruth_: experiment ID, position X, position Y, orientation (of each robot's true pose) 12 | * _measurement_: experiment ID, observed landmark ID, observed distance, observed bearing (from each robot's true pose to a landmark) 13 | 14 | #### Reference 15 | * [1] K. Leung, Y. Halpern, T. Barfoot, and H. Liu, __The UTIAS Multi-Robot Cooperative Localization and Mapping Dataset__, The International Journal of Robotics Research, Vol. 30, No. 8, 2011 [IJRR Online](http://ijr.sagepub.com/content/30/8/969) [Website](http://asrl.utias.utoronto.ca/datasets/mrclam/) 16 | -------------------------------------------------------------------------------- /observe_pose.m: -------------------------------------------------------------------------------- 1 | function [obsData, obsMap] = observe_pose(map, pose, visibleRate) 2 | %OBSERVE_POSE Measure pose from the given pose to landmarks. 3 | % 4 | % [OBS_DATA, OBS_MAP] = OBSERVE_POSE(MAP, POSE, VISIBLE_RATE) 5 | % (matrix) MAP : A landmark map (Nx6 matrix) 6 | % (matrix) POSE : Pose of the target object (1x6 matrix) 7 | % (scalar) VISIBLE_RATE: Visible probability of landmarks (default: 1) 8 | % (matrix) OBS_DATA : The measured pose from POSE to landmarks (Mx6 matrix) 9 | % (matrix) OBS_MAP : The landmark map of measured landmarks (Mx6 matrix) 10 | % 11 | % Note: Pose of an object, POSE, is represented by 1x6 vector whose first three 12 | % columns represent position of the object, (x, y, z), and last three 13 | % columns represent orientation of the object, (r_x, r_y, r_z) [rad]. 14 | % 15 | % Note: A landmark map, MAP, is Nx6 matrix which contains position and 16 | % orientation of landmarks in the world coordinate. Its first three columns 17 | % represents position of landmarks, (x, y, z). Its last three columns represent 18 | % orientation of landmarks, (r_x, r_y, r_z) [rad]. 19 | % 20 | % Note: The number of output data, M, will be approximately VISIBLE_RATE * N. 21 | % If there is no visible landmark, OBS_DATA and OBS_MAP will be an empty matrix. 22 | % Please use the command, ISEMPTY, to identify an empty matrix. 23 | % 24 | % Note: The measured pose, OBS_DATA, is represented by Mx6 matrix whose 25 | % format is exactly same with POSE and MAP. 26 | % 27 | % Example: 28 | % map = [ 0, 0, 5, 0, 0, 0; ... 29 | % 5, 0, 5, 0, 0, 0; ... 30 | % 5, 5, 5, 0, 0, 0; ... 31 | % 0, 5, 5, 0, 0, 0 ]; 32 | % pose = [ 3, 2, 9, 0, 0, pi / 2 ]; 33 | % [obsData, obsMap] = observe_displacement(map, pose) 34 | % 35 | % See also observe_distance, observe_distance_relative, observe_bearing, observe_displacement. 36 | 37 | if (nargin < 3) || isempty(visibleRate) 38 | visibleRate = 1; 39 | end 40 | 41 | isVisible = rand(size(map,1), 1) < visibleRate; % Select visible landmarks 42 | obsMap = map(isVisible,:); 43 | obsNum = size(obsMap,1); 44 | obsDim = 6; 45 | 46 | obsData = zeros(obsNum,obsDim); 47 | if obsNum > 0 48 | delta = obsMap(:,1:3) - repmat(pose(1:3), obsNum, 1); 49 | obsData(:,1:3) = delta * tran_rad2rot(pose(4:6)); % Calculate displacement 50 | % a = R' * b --> a' = b' * R 51 | for i = 1:obsNum 52 | R = tran_rad2rot(pose(4:6))' * tran_rad2rot(obsMap(i,4:6)); % Calculate orientation one by one 53 | obsData(i,4:6) = tran_rot2rad(R); 54 | end 55 | end 56 | -------------------------------------------------------------------------------- /observe_bearing.m: -------------------------------------------------------------------------------- 1 | function [obsData, obsMap] = observe_bearing(map, pose, visibleRate) 2 | %OBSERVE_BEARING Measure bearing angle from the given pose to landmarks. 3 | % 4 | % [OBS_DATA, OBS_MAP] = OBSERVE_BEARING(MAP, POSE, VISIBLE_RATE) 5 | % (matrix) MAP : A landmark map (Nx6 matrix) 6 | % (matrix) POSE : Pose of the target object (1x6 matrix) 7 | % (scalar) VISIBLE_RATE: Visible probability of landmarks (default: 1) 8 | % (matrix) OBS_DATA : The measured bearing from POSE to landmarks (Mx2 matrix) 9 | % (matrix) OBS_MAP : The landmark map of measured landmarks (Mx6 matrix) 10 | % 11 | % Note: Pose of an object, POSE, is represented by 1x6 vector whose first three 12 | % columns represent position of the object, (x, y, z), and last three 13 | % columns represent orientation of the object, (r_x, r_y, r_z) [rad]. 14 | % 15 | % Note: A landmark map, MAP, is Nx6 matrix which contains position and 16 | % orientation of landmarks in the world coordinate. Its first three columns 17 | % represents position of landmarks, (x, y, z). Its last three columns represent 18 | % orientation of landmarks, (r_x, r_y, r_z) [rad]. 19 | % 20 | % Note: The number of output data, M, will be approximately VISIBLE_RATE * N. 21 | % If there is no visible landmark, OBS_DATA and OBS_MAP will be an empty matrix. 22 | % Please use the command, ISEMPTY, to identify an empty matrix. 23 | % 24 | % Note: The measured bearing angle, OBS_DATA, is represented by Mx2 matrix whose i-th row 25 | % is observed angle (azimuthal angle, polar angle) of i-th landmark in OBS_MAP with 26 | % respect to the given pose, POSE. 27 | % 28 | % References: 29 | % [1] Spherical Coordinate System, Wikipedia 30 | % URL: http://en.wikipedia.org/wiki/Spherical_coordinate_system 31 | % 32 | % Examples: 33 | % map = [ 0, 0, 5, 0, 0, 0; ... 34 | % 5, 0, 5, 0, 0, 0; ... 35 | % 5, 5, 5, 0, 0, 0; ... 36 | % 0, 5, 5, 0, 0, 0 ]; 37 | % pose = [ 3, 2, 9, 0, 0, pi / 2 ]; 38 | % [obsData, obsMap] = observe_bearing(map, pose) 39 | % 40 | % See also observe_distance, observe_distance_relative, observe_displacement, observe_pose. 41 | 42 | if nargin < 3 43 | visibleRate = 1; 44 | end 45 | 46 | isVisible = rand(size(map,1), 1) < visibleRate; % Select visible landmarks 47 | obsMap = map(isVisible,:); 48 | obsNum = size(obsMap,1); 49 | obsDim = 2; 50 | 51 | obsData = zeros(obsNum,obsDim); 52 | if obsNum > 0 53 | delta = obsMap(:,1:3) - repmat(pose(1:3), obsNum, 1); 54 | delta = delta * tran_rad2rot(pose(4:6)); % a = R' * b --> a' = b' * R 55 | r = sqrt(delta(:,1).^2 + delta(:,2).^2 + delta(:,3).^2); 56 | obsData(:,1) = atan2(delta(:,2), delta(:,1)); % Calculate azimuthal angle 57 | obsData(:,2) = asin(delta(:,3) ./ r); % Calculate polar angle 58 | end 59 | -------------------------------------------------------------------------------- /localize2d_shimshoni02_algebraic.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_shimshoni02_algebraic(data, map) 2 | %LOCALIZE2D_SHIMSHONI02_ALGEBRAIC Estimate 2D position and orientation using bearing 3 | % angles measured from landmarks (N >= 3) 4 | % 5 | % [POSE, VALID] = LOCALIZE2D_SHIMSHONI02_ALGEBRAIC(DATA, MAP) 6 | % (matrix) DATA : The measured bearing angles from landmarks (Nx2 matrix) 7 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 8 | % (matrix) POSE : The estimated pose (1x6 matrix) 9 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 10 | % 11 | % Note: Please refer to the command, OBSERVE_BEARING, for the convention of DATA, 12 | % MAP, and POSE. 13 | % 14 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 15 | % element of POSE. Since this algorithm estimates 2D position and orientation, 16 | % the expected VALID is [true, true, false, false, false, true]. 17 | % 18 | % Note: This implementation is based on Shimshoni's basic algebraic approach [1] 19 | % without the proposed improvement. 20 | % 21 | % Reference: 22 | % [1] I. Shimshoni, On Mobile Robot Localization from Landmark Bearings, 23 | % IEEE Transactions on Robotics and Automation, Vol. 18, No. 6, 2002 24 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1159015 25 | % 26 | % Example: 27 | % N = 3; 28 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 29 | % data = [2 * pi * rand(N,1) - pi, zeros(N,1)]; % Random measurement 30 | % [pose, valid] = localize2d_shimshoni02_algebraic(data, map) 31 | % 32 | % See also localize2d_shimshoni02_improved, localize2d_betke97. 33 | 34 | if size(data,1) < 3 35 | error('DATA has less number of observations!'); 36 | end 37 | if size(data,2) ~= 2 38 | error('DATA has wrong size!'); 39 | end 40 | 41 | % Calculate pose using algebraic method (Section II.C) 42 | if sum(data(:,1) == 0) > 0 % A degenerate case of cotangent 43 | pose = zeros(1,6); 44 | valid = false * ones(1,6); 45 | return; 46 | end 47 | A = ... 48 | [ ... 49 | map(:,1) - map(:,2) .* cot(data(:,1)), ... 50 | map(:,2) + map(:,1) .* cot(data(:,1)), ... 51 | ones(size(data,1),1), ... 52 | -cot(data(:,1)) ... 53 | ]; 54 | [U,S,V] = svd(A); 55 | W = V(:,end); 56 | W = W ./ norm(W(1:2)); % Scaling 57 | R = [W(1), W(2); -W(2), W(1)]; 58 | T = W(3:4); 59 | P = -R' * T; % T = -R * P 60 | 61 | % Select one betweetn two solutions using the first landmark (i = 1) 62 | P_i = map(1,1:2)'; 63 | l = norm(P - P_i); 64 | signL = sign(l * [cos(data(1,1)); sin(data(1,1))]); 65 | signR = sign(R * P_i + T); 66 | if ~isequal(signL, signR) 67 | W = -W; 68 | R = [W(1), W(2); -W(2), W(1)]; 69 | T = W(3:4); 70 | P = -R' * T; 71 | end 72 | 73 | pose = [P(1), P(2), 0, 0, 0, atan2(W(2), W(1))]; 74 | valid = [true, true, false, false, false, true]; 75 | -------------------------------------------------------------------------------- /run_example.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== A Simple Example for Triangulation Toolbox =='); 5 | 6 | % The given configuration: a landmark map and true pose 7 | trueMap = ... 8 | [ ... 9 | % x, y, z, r_x, r_y, r_z ... 10 | 0, 0, 0, 0, 0, 0; ... 11 | 4, 0, 0, 0, 0, 0; ... 12 | 8, 0, 0, 0, 0, tran_deg2rad( +90); ... 13 | 8, 4, 0, 0, 0, tran_deg2rad( +90); ... 14 | 8, 8, 0, 0, 0, tran_deg2rad(-180); ... 15 | 4, 8, 0, 0, 0, tran_deg2rad(-180); ... 16 | 0, 8, 0, 0, 0, tran_deg2rad( -90); ... 17 | 0, 4, 0, 0, 0, tran_deg2rad( -90); ... 18 | ]; 19 | truePose = [3, 2, 0, 0, 0, pi / 9]; 20 | paramVisibility = 0.8; 21 | paramNoise = 0.05; 22 | 23 | % Generate observation with Gaussian noise 24 | [obsData, obsMap] = observe_bearing(trueMap, truePose, paramVisibility); 25 | obsData = apply_noise_gauss(obsData, paramNoise); 26 | 27 | % Estimate pose from observation 28 | [pose, valid] = localize2d_shimshoni02_algebraic(obsData, obsMap); 29 | errPos = error_position(truePose(1:3), pose(1:3)); 30 | errOri = error_orientation(truePose(4:6), pose(4:6)); 31 | 32 | % Print results 33 | disp('==== The Given Configuration ===='); 34 | fprintf('- The true position : (%.3f, %.3f, %.3f)\n', truePose(1:3)); 35 | fprintf('- The true orientation: (%.3f, %.3f, %.3f) [rad] /', truePose(4:6)); 36 | fprintf(' (%.1f, %.1f, %.1f) [deg]\n\n', tran_rad2deg(truePose(4:6))); 37 | disp('==== The Estimated Pose ===='); 38 | fprintf('- The estimated position : (%.3f, %.3f, %.3f)\n', pose(1:3)); 39 | fprintf('- The estimated orientation: (%.3f, %.3f, %.3f) [rad] /', pose(4:6)); 40 | fprintf(' (%.1f, %.1f, %.1f) [deg]\n', tran_rad2deg(pose(4:6))); 41 | fprintf('- The position error : %.3f\n', errPos); 42 | fprintf('- The orientation error: %.3f [rad] /', errOri); 43 | fprintf(' %.1f [deg]\n', tran_rad2deg(errOri)); 44 | 45 | % Visualize results 46 | figure('Color', [1, 1, 1]); 47 | hold on; 48 | plot3(trueMap(:,1), trueMap(:,2), trueMap(:,3), 'b.', 'MarkerSize', 20); % All landmarks 49 | plot3(obsMap(:,1), obsMap(:,2), obsMap(:,3), 'ro', 'LineWidth', 2); % The observed landmarks 50 | plot3(truePose(1), truePose(2), truePose(3), 'b.', 'MarkerSize', 30); % The true postion 51 | plot3(pose(1), pose(2), pose(3), 'go', 'LineWidth', 3, 'MarkerSize', 10); % The estimated position 52 | R = tran_rad2rot(truePose(4:6)); 53 | arrow = [truePose(1:3); truePose(1:3) + R(:,1)']; 54 | plot3(arrow(:,1), arrow(:,2), arrow(:,3), 'b-', 'LineWidth', 2); % The true orientation 55 | R = tran_rad2rot(pose(4:6)); 56 | arrow = [pose(1:3); pose(1:3) + R(:,1)']; 57 | plot3(arrow(:,1), arrow(:,2), arrow(:,3), 'g-', 'LineWidth', 2); % The estimated orientation 58 | title('Visualization for Triangulation Toolbox'); 59 | xlabel('X'); 60 | ylabel('Y'); 61 | zlabel('Z'); 62 | legend({'All Landmarks', 'Obs. Landmarks', 'True Position', 'Est. Position'}); 63 | box on; 64 | grid on; 65 | axis equal; 66 | hold off; 67 | -------------------------------------------------------------------------------- /run_draw_distribution.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Position/Error Distribution for Triangulation Toolbox =='); 5 | 6 | % Configure the drawing target %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | target.file = 'run_eval_random.mat'; 8 | target.ex = 1; % Index of experiment to analyze in detail 9 | target.v = 6; % Index of variable to analyze in detail 10 | histogram.scale = 6; % The scale of histogram (width = scale * median) 11 | histogram.bin = 50; % The number of bins for histogram 12 | 13 | % Draw distributions (position, position error, orientation error) %%%%%%%%%%%% 14 | % Load the target evaluation file 15 | load(target.file); 16 | 17 | % Draw distribution of estimated position with the ground truth 18 | figure('Color', [1, 1, 1]); 19 | hold on; 20 | set(gca, 'FontSize', 12); 21 | box on; 22 | grid on; 23 | for m = config.algoSelM 24 | plot3(record.pose{target.ex,target.v}(:,1,m), record.pose{target.ex,target.v}(:,2,m), ... 25 | record.pose{target.ex,target.v}(:,3,m), [config.algorithm{m,config.algoLine}(1), '.']) 26 | end 27 | for i = 1:size(config.pose,1) 28 | truth = config.pose(i,1:3); 29 | line(get(gca, 'XLim'), [truth(2), truth(2)], [truth(3), truth(3)], 'Color', 'y', 'LineWidth', 1); 30 | line([truth(1), truth(1)], get(gca, 'YLim'), [truth(3), truth(3)], 'Color', 'y', 'LineWidth', 1); 31 | line([truth(1), truth(1)], [truth(2), truth(2)], get(gca, 'ZLim'), 'Color', 'y', 'LineWidth', 1); 32 | end 33 | xlabel('X [m]', 'FontSize', 12); 34 | ylabel('Y [m]', 'FontSize', 12); 35 | zlabel('Z [m]', 'FontSize', 12); 36 | legend(config.algorithm(config.algoSelM,config.algoName), 'FontSize', 12); 37 | hold off; 38 | 39 | % Draw distribution of position/orientation error (CDF) 40 | xtext = { 'distance [m]', 'angle [deg]', 'angle [deg]' }; 41 | ytext = { 'P(position error < distance)', 'P(orientation error < angle)', 'P(reprojection error < angle)' }; 42 | for cr = 1:(size(criteria.name, 2) - 2) 43 | med = median(median(record.perf{target.ex,target.v}(:,cr,config.algoSelM))); 44 | if med == 0, med = 1; end 45 | bins = 0:(histogram.scale*med/histogram.bin):histogram.scale*med; 46 | isDrawn = []; 47 | figure('Color', [1, 1, 1]); 48 | hold on; 49 | set(gca, 'FontSize', 12); 50 | box on; 51 | grid on; 52 | for m = config.algoSelM 53 | if (cr == 2) && (config.algorithm{m,config.algoVald}(end) == 0), continue; end 54 | N = length(record.perf{target.ex,target.v}(:,cr,m)); 55 | cdfN = histc(record.perf{target.ex,target.v}(:,cr,m), bins) / N; 56 | for i = 2:size(cdfN) 57 | cdfN(i) = cdfN(i-1) + cdfN(i); 58 | end 59 | plot(bins, cdfN, config.algorithm{m,config.algoLine}, 'LineWidth', 2, 'MarkerSize', 2); 60 | isDrawn = [isDrawn, m]; 61 | end 62 | axis([0, histogram.scale*med, 0, 1]); 63 | xlabel(xtext{cr}, 'FontSize', 12); 64 | ylabel(ytext{cr}, 'FontSize', 12); 65 | legend(config.algorithm(isDrawn,config.algoName), 'FontSize', 12); 66 | hold off; 67 | end 68 | -------------------------------------------------------------------------------- /localize2d_shimshoni02_improved.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_shimshoni02_improved(data, map) 2 | %LOCALIZE2D_SHIMSHONI02_IMPROVED Estimate 2D position and orientation using bearing 3 | % angles measured from landmarks (N >= 3) 4 | % 5 | % [POSE, VALID] = LOCALIZE2D_SHIMSHONI02_IMPROVED(DATA, MAP) 6 | % (matrix) DATA : The measured bearing angles from landmarks (Nx2 matrix) 7 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 8 | % (matrix) POSE : The estimated pose (1x6 matrix) 9 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 10 | % 11 | % Note: Please refer to the command, OBSERVE_BEARING, for the convention of DATA, 12 | % MAP, and POSE. 13 | % 14 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 15 | % element of POSE. Since this algorithm estimates 2D position and orientation, 16 | % the expected VALID is [true, true, false, false, false, true]. 17 | % 18 | % Note: This implementation is based on Shimshoni's algebraic approach with the first 19 | % and second improvements, bounded variance and scale data. 20 | % 21 | % Reference: 22 | % [1] I. Shimshoni, On Mobile Robot Localization from Landmark Bearings, 23 | % IEEE Transactions on Robotics and Automation, Vol. 18, No. 6, 2002 24 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1159015 25 | % 26 | % Example: 27 | % N = 3; 28 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 29 | % data = [2 * pi * rand(N,1) - pi, zeros(N,1)]; % Random measurement 30 | % [pose, valid] = localize2d_shimshoni02_improved(data, map) 31 | % 32 | % See also localize2d_shimshoni02_algebraic, localize2d_betke97. 33 | 34 | if size(data,1) < 3 35 | error('DATA has less number of observations!'); 36 | end 37 | if size(data,2) ~= 2 38 | error('DATA has wrong size!'); 39 | end 40 | 41 | % Calculate pose using algebraic method (Section III) 42 | C = mean(map(:,1:2)); 43 | delta = map(:,1:2) - repmat(C, size(map,1), 1); 44 | SZ = max(sqrt(delta(:,1).^2 + delta(:,2).^2)); 45 | map = (map(:,1:2) - repmat(C, size(map,1), 1)) / SZ; % 2nd improvement, scaled data 46 | A = ... 47 | [ ... 48 | map(:,1) .* sin(data(:,1)) - map(:,2) .* cos(data(:,1)), ... 49 | map(:,2) .* sin(data(:,1)) + map(:,1) .* cos(data(:,1)), ... 50 | sin(data(:,1)), ... 51 | -cos(data(:,1)) ... 52 | ]; % 1st improvement, bounded variance 53 | [U,S,V] = svd(A); 54 | W = V(:,end); 55 | W = W ./ norm(W(1:2)); % Scaling 56 | R = [W(1), W(2); -W(2), W(1)]; 57 | T = W(3:4); 58 | P = -R' * T; % T = -R * P 59 | 60 | % Select one betweetn two solutions using the first landmark (i = 1) 61 | P_i = map(1,1:2)'; 62 | l = norm(P - P_i); 63 | signL = sign(l * [cos(data(1,1)); sin(data(1,1))]); 64 | signR = sign(R * P_i + T); 65 | if ~isequal(signL, signR) 66 | W = -W; 67 | R = [W(1), W(2); -W(2), W(1)]; 68 | T = W(3:4); 69 | P = -R' * T; 70 | end 71 | 72 | P = P * SZ + C'; 73 | pose = [P(1), P(2), 0, 0, 0, atan2(W(2), W(1))]; 74 | valid = [true, true, false, false, false, true]; 75 | -------------------------------------------------------------------------------- /localize2d_betke97.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize2d_betke97(data, map) 2 | %LOCALIZE2D_BETKE97 Estimate 2D position and orientation using bearing angles 3 | % measured from landmarks (N >= 3) 4 | % 5 | % [POSE, VALID] = LOCALIZE2D_BETKE97(DATA, MAP) 6 | % (matrix) DATA : The measured bearing angles from landmarks (Nx2 matrix) 7 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 8 | % (matrix) POSE : The estimated pose (1x6 matrix) 9 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix) 10 | % 11 | % Note: Please refer to the command, OBSERVE_BEARING, for the convention of DATA, 12 | % MAP, and POSE. 13 | % 14 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 15 | % element of POSE. Since this algorithm estimates 2D position and orientation, 16 | % the expected VALID is [true, true, false, false, false, true]. 17 | % 18 | % Reference: 19 | % [1] M. Betke and L. Gurvits, Mobile Robot Localization using Landmarks, 20 | % IEEE Transactions on Robotics and Automation, Vol. 13, No. 2, 1997 21 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=563647 22 | % 23 | % Example: 24 | % N = 3; 25 | % map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map 26 | % data = [2 * pi * rand(N,1) - pi, zeros(N,1)]; % Random measurement 27 | % [pose, valid] = localize2d_betke97(data, map) 28 | % 29 | % See also localize2d_shimshoni02_algebraic, localize2d_shimshoni02_improved. 30 | 31 | if size(data,1) < 3 32 | error('DATA has less number of observations!'); 33 | end 34 | if size(data,2) ~= 2 35 | error('DATA has wrong size!'); 36 | end 37 | 38 | n = size(data,1); 39 | z0 = map(n,1:2); 40 | z = map(1:n-1,1:2); 41 | tau0 = data(n,1); 42 | psi = data(1:n-1,1) - tau0; 43 | n = n - 1; 44 | 45 | % 1. Initialize 46 | v = z - repmat(z0, n, 1); % v_i = z_0 - z_i 47 | c = [+v(:,1) ./ (v(:,1).^2 + v(:,2).^2), -v(:,2) ./ (v(:,1).^2 + v(:,2).^2)]; 48 | % c_i = 1 / v_i 49 | sum_of_c = sum(c); 50 | b = [c(:,1) .* cos(psi) - c(:,2) .* sin(psi), c(:,1) .* sin(psi) + c(:,2) .* cos(psi)]; 51 | % b_i = c_i * exp(j * psi_i) 52 | 53 | % 2. Calculate s = 1 / 2 * A' * c 54 | s = n * sum(b .* c, 2) - sum(b .* repmat(sum_of_c, n, 1), 2); 55 | 56 | % 3. Calculate r = (A'*A)^-1 * A' 57 | r = RatiosCalculator(b, s); 58 | 59 | % 4. Calculate robot position, p 60 | d = repmat(r, 1, 2) .* b - c; 61 | zr0 = [+d(:,1) ./ (d(:,1).^2 + d(:,2).^2), -d(:,2) ./ (d(:,1).^2 + d(:,2).^2)]; 62 | % zr0_i = 1 / (r_i * b_i - c_i) 63 | p = repmat(z0, n, 1) - zr0; % p_i = z_0 - zr0_i 64 | p = sum(p) / n; 65 | 66 | % 5. Calculate robot orientation, theta 67 | zr0 = z0 - p; 68 | theta = trim_rad(atan2(zr0(2), zr0(1)) - tau0); 69 | 70 | pose = [p, 0, 0, 0, theta]; 71 | valid = [true, true, false, false, false, true]; 72 | 73 | 74 | 75 | function [r] = RatiosCalculator(b, s) 76 | n = size(s, 1); 77 | 78 | % 1. Compute diagonal matrix (n * D)^-1 79 | D = diag(b(:,1).^2 + b(:,2).^2); 80 | Dinv = (n * D)^-1; 81 | Dinv_b = Dinv * b; 82 | 83 | % 2. Compute K^-1 * s 84 | Kinv_s = Dinv*s + Dinv_b(:,1)*Dinv_b(:,1)'*s / (1 - Dinv_b(:,1)'*b(:,1)); 85 | 86 | % 3. Compute K^-1 * b_y 87 | Kinv_b = Dinv_b(:,2) + Dinv_b(:,1)*Dinv_b(:,1)'*b(:,2) / (1 - Dinv_b(:,1)'*b(:,1)); 88 | 89 | % 4. Compute ratio vector r 90 | r = Kinv_s + (Kinv_b * (Kinv_b'*s)) / (1 - Kinv_b'*b(:,2)); 91 | r(r < 0) = 0; 92 | -------------------------------------------------------------------------------- /localize3d_thomas05.m: -------------------------------------------------------------------------------- 1 | function [pose, valid] = localize3d_thomas05(data, map) 2 | %LOCALIZE3D_THOMAS05 Estimate 3D position using distances measured from landmarks (N >= 3) 3 | % 4 | % [POSE, VALID] = LOCALIZE3D_THOMAS05(DATA, MAP) 5 | % (matrix) DATA : The measured distances from landmarks (Nx1 matrix) 6 | % (matrix) MAP : The corresponding landmark map (Nx6 matrix) 7 | % (matrix) POSE : The estimated pose (1x6 matrix, 2x6 matrix) 8 | % (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix, 2x6 matrix) 9 | % 10 | % Note: Please refer to the command, OBSERVE_DISTANCE, for the convention of DATA, 11 | % MAP, and POSE. 12 | % 13 | % Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each 14 | % element of POSE. Since this algorithm estimates 3D position, the expected 15 | % VALID is [true, true, true, false, false, false]. 16 | % 17 | % Note: This algorithm uses only first three measurements. Even though the number of measurements, 18 | % N, are more than three, it does not optimize position in least-squares sense. If N is 19 | % exactly three, this algorithm may return two sets of POSE and VALID due to ambiguity. 20 | % If N is more than three, this algorithm uses 4-th measurement to resolve ambiguity. 21 | % 22 | % Note: This implementation is slightly modified from Thomas's original code available at 23 | % his homepage, http://www.iri.upc.edu/people/thomas/. 24 | % 25 | % Reference: 26 | % [1] F. Thomas and L. Ros, Revisiting Trilateration for Robot Localization, 27 | % IEEE Transactions on Robotics, Vol. 21, No. 1, 2005 28 | % URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1391018 29 | % 30 | % Example: 31 | % N = 4; 32 | % map = [10 * rand(N,3), zeros(N,3)]; % Random 2D landmark map 33 | % data = 10 * rand(N,1); % Random measurement 34 | % [pose, valid] = localize3d_thomas05(data, map) 35 | % 36 | % See also localize3d_sayed05_toa. 37 | 38 | if size(data,1) < 3 39 | error('DATA has less number of observations!'); 40 | elseif size(data,1) > 3 41 | warning('The algorithm uses the first three observations!'); 42 | end 43 | if size(data,2) ~= 1 44 | error('DATA has wrong size!'); 45 | end 46 | 47 | p1 = map(1,1:3)'; 48 | p2 = map(2,1:3)'; 49 | p3 = map(3,1:3)'; 50 | l1 = data(1); 51 | l2 = data(2); 52 | l3 = data(3); 53 | 54 | a = norm(p2 - p1); 55 | b = norm(p3 - p1); 56 | c = norm(p3 - p2); 57 | 58 | area = -0.25 * det([[0, 1, 1, 1]; 59 | [1, 0, a*a, b*b]; 60 | [1, a*a, 0, c*c]; 61 | [1, b*b, c*c, 0]]); 62 | 63 | volumen = 0.125 * det([[0, 1, 1, 1, 1]; 64 | [1, 0, a*a, b*b, l1*l1]; 65 | [1, a*a, 0, c*c, l2*l2]; 66 | [1, b*b, c*c, 0, l3*l3]; 67 | [1, l1*l1, l2*l2, l3*l3, 0]]); 68 | 69 | if (area == 0) || (volumen < 0) 70 | warning('A degernerate case was detected!'); 71 | 72 | pose = zeros(1,6); 73 | valid = [false, false, false, false, false, false]; 74 | else 75 | k1 = (+0.25 / area) * det([[0, 1, 1, 1]; 76 | [1, 0, b*b, l1*l1]; 77 | [1, a*a, c*c, l2*l2]; 78 | [1, b*b, 0, l3*l3]]); 79 | 80 | k2 = (-0.25 / area) * det([[0, 1, 1, 1]; 81 | [1, 0, a*a, l1*l1]; 82 | [1, a*a, 0, l2*l2]; 83 | [1, b*b, c*c, l3*l3]]); 84 | 85 | k3 = sqrt(volumen) / area; 86 | 87 | p4a = p1 + k1 * (p2 - p1) + k2 * (p3 - p1) + k3 * cross(p2 - p1, p3 - p1); 88 | p4b = p1 + k1 * (p2 - p1) + k2 * (p3 - p1) - k3 * cross(p2 - p1, p3 - p1); 89 | 90 | pose = [p4a', 0, 0, 0]; 91 | valid = [true, true, true, false, false, false]; 92 | if ~isequal(p4a, p4b) 93 | pose = [pose; p4b', 0, 0, 0]; 94 | valid = [valid; valid]; 95 | end 96 | end; 97 | -------------------------------------------------------------------------------- /run_test_localize.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Unit-test of Algorithms for Triangulation Toolbox =='); 5 | 6 | % The given landmark map and true pose for 2D localization %%%%%%%%%%%%%%%%%%%% 7 | trueMap = ... 8 | [ ... 9 | % x, y, z, r_x, r_y, r_z ... 10 | 5, 0, 0, 0, 0, tran_deg2rad( +90); ... 11 | 5, 5, 0, 0, 0, tran_deg2rad(-180); ... 12 | 0, 5, 0, 0, 0, tran_deg2rad( -90); ... 13 | 0, 0, 0, 0, 0, 0; ... 14 | ]; 15 | truePose = [3, 2, 0, 0, 0, pi / 9]; 16 | 17 | % localize2d_sayed05_toa 18 | disp('==== localize2d_sayed05_toa ===='); 19 | [obsData, obsMap] = observe_distance(trueMap, truePose); 20 | [pose, valid] = localize2d_sayed05_toa(obsData, obsMap); 21 | test_is_true(valid == [1, 1, 0, 0, 0, 0]); 22 | test_is_near(pose(valid == true), truePose(valid == true)); 23 | 24 | % localize2d_sayed05_tdoa 25 | disp('==== localize2d_sayed05_tdoa ===='); 26 | [obsData, obsMap] = observe_distance_relative(trueMap, truePose); 27 | [pose, valid] = localize2d_sayed05_tdoa(obsData, obsMap); 28 | test_is_true(valid == [1, 1, 0, 0, 0, 0]); 29 | test_is_near(pose(valid == true), truePose(valid == true)); 30 | 31 | % localize2d_betke97 32 | disp('==== localize2d_betke97 ===='); 33 | [obsData, obsMap] = observe_bearing(trueMap, truePose); 34 | [pose, valid] = localize2d_betke97(obsData, obsMap); 35 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 36 | test_is_near(pose(valid == true), truePose(valid == true)); 37 | 38 | % localize2d_shimshoni02_algebraic 39 | disp('==== localize2d_shimshoni02_algebraic ===='); 40 | [obsData, obsMap] = observe_bearing(trueMap, truePose); 41 | [pose, valid] = localize2d_shimshoni02_algebraic(obsData, obsMap); 42 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 43 | test_is_near(pose(valid == true), truePose(valid == true)); 44 | 45 | % localize2d_shimshoni02_improved 46 | disp('==== localize2d_shimshoni02_improved ===='); 47 | [obsData, obsMap] = observe_bearing(trueMap, truePose); 48 | [pose, valid] = localize2d_shimshoni02_improved(obsData, obsMap); 49 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 50 | test_is_near(pose(valid == true), truePose(valid == true)); 51 | 52 | % localize2d_se05 53 | disp('==== localize2d_se05 ===='); 54 | [obsData, obsMap] = observe_displacement(trueMap, truePose); 55 | [pose, valid] = localize2d_se05(obsData, obsMap); 56 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 57 | test_is_near(pose(valid == true), truePose(valid == true)); 58 | 59 | % localize2d_sayed05_aoa 60 | disp('==== localize2d_sayed05_aoa ===='); 61 | [obsData, obsMap] = observe_displacement(trueMap, truePose); 62 | [pose, valid] = localize2d_sayed05_aoa(obsData, obsMap); 63 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 64 | test_is_near(pose(valid == true), truePose(valid == true)); 65 | 66 | % localize2d_pose 67 | disp('==== localize2d_pose ===='); 68 | [obsData, obsMap] = observe_pose(trueMap, truePose); 69 | [pose, valid] = localize2d_pose(obsData, obsMap); 70 | test_is_true(valid == [1, 1, 0, 0, 0, 1]); 71 | test_is_near(pose(valid == true), truePose(valid == true)); 72 | 73 | % The given landmark map and true pose for 3D localization %%%%%%%%%%%%%%%%%%%% 74 | trueMap = ... 75 | [ ... 76 | % x, y, z, r_x, r_y, r_z ... 77 | 5, 0, 5, 0, 0, tran_deg2rad( +90); ... 78 | 5, 5, 0, 0, 0, tran_deg2rad(-180); ... 79 | 0, 5, 5, 0, 0, tran_deg2rad( -90); ... 80 | 0, 0, 5, 0, 0, 0 ... 81 | ]; 82 | truePose = [3, 2, 1, 0, 0, pi / 9]; 83 | 84 | % localize_sayed05_toa 85 | disp('==== localize_sayed05_toa ===='); 86 | [obsData, obsMap] = observe_distance(trueMap, truePose); 87 | [pose, valid] = localize3d_sayed05_toa(obsData, obsMap); 88 | test_is_true(valid == [1, 1, 1, 0, 0, 0]); 89 | test_is_near(pose(valid == true), truePose(valid == true)); 90 | 91 | % localize_thomas05 92 | disp('==== localize_thomas05 ===='); 93 | [obsData, obsMap] = observe_distance(trueMap, truePose); 94 | [pose, valid] = localize3d_thomas05(obsData, obsMap); 95 | for i = 1:size(pose,1) 96 | test_is_true(valid(i,:) == [1, 1, 1, 0, 0, 0]); 97 | test_is_near(pose(i,valid(i,:) == true), truePose(valid(i,:) == true)); 98 | end 99 | -------------------------------------------------------------------------------- /run_eval_roh.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Localization Evaluatioin (with Roh'' Angulation Dataset) for Triangulation Toolbox =='); 5 | 6 | % Configure experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | config.mapFile = 'dataset_roh/landmark.csv'; % A map file 8 | config.dataFile = ... % A list of data files 9 | { ... 10 | 'dataset_roh/(X1.5,Y1.5).csv'; ... 11 | 'dataset_roh/(X1.5,Y3.0).csv'; ... 12 | 'dataset_roh/(X1.5,Y4.5).csv'; ... 13 | 'dataset_roh/(X3.0,Y1.5).csv'; ... 14 | 'dataset_roh/(X3.0,Y3.0).csv'; ... 15 | 'dataset_roh/(X3.0,Y4.5).csv'; ... 16 | 'dataset_roh/(X4.5,Y1.5).csv'; ... 17 | 'dataset_roh/(X4.5,Y3.0).csv'; ... 18 | 'dataset_roh/(X4.5,Y4.5).csv'; ... 19 | }; 20 | config.pose = ... % A list of ground truth 21 | [ ... 22 | 1.50, 1.50, 0, 0, 0, tran_deg2rad(90.0); ... 23 | 1.50, 3.00, 0, 0, 0, tran_deg2rad(90.0); ... 24 | 1.50, 4.50, 0, 0, 0, tran_deg2rad(90.0); ... 25 | 3.00, 1.50, 0, 0, 0, tran_deg2rad(90.0); ... 26 | 3.00, 3.00, 0, 0, 0, tran_deg2rad(90.0); ... 27 | 3.00, 4.50, 0, 0, 0, tran_deg2rad(90.0); ... 28 | 4.50, 1.50, 0, 0, 0, tran_deg2rad(90.0); ... 29 | 4.50, 3.00, 0, 0, 0, tran_deg2rad(90.0); ... 30 | 4.50, 4.50, 0, 0, 0, tran_deg2rad(90.0); ... 31 | ]; 32 | config.algorithm = ... % Description of localization algorithms 33 | { ... 34 | % #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ... 35 | 1, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ... 36 | 2, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ... 37 | 3, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ... 38 | }; 39 | config.matFile = 'run_eval_roh.mat'; 40 | 41 | criteria.name = {'Position Error [m]', 'Orientation Error [deg]', 'Reprojection Error [deg]', ... 42 | 'Computing Time [msec]', 'Number of Failures'}; % Name of evaluation criteria 43 | criteria.repr = {@median, @median, @median, @median, @sum}; % Functions for calculating representive values 44 | % (e.g. mean, median, std, and sum) 45 | criteria.format = {'%.6f', '%.3f', '%.3f', '%.6f', '%d'}; % Format for printing text 46 | 47 | % Perform experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 48 | config.algoDims = 2; 49 | config.algoName = 3; 50 | config.algoEstm = 4; 51 | config.algoObsv = 5; 52 | config.algoMinN = 6; 53 | config.algoVald = 7; 54 | config.algoLine = 8; 55 | config.algoSelM = 1:size(config.algorithm,1); 56 | 57 | % 1. Read the map file 58 | map = load(config.mapFile); 59 | map = [map, zeros(size(map,1),4)]; 60 | 61 | record.perf{1,1} = []; 62 | record.pose{1,1} = []; 63 | for d = 1:size(config.dataFile,1) 64 | % 2. Read each data file 65 | data = tran_deg2rad(load(config.dataFile{d}))'; 66 | truth = config.pose(d,:); 67 | for t = 1:size(data,2) 68 | obsData = [data(:,t), zeros(size(data,1),1)]; 69 | set.perf = zeros(1,4,size(config.algorithm,1)); 70 | set.pose = zeros(1,6,size(config.algorithm,1)); 71 | for m = config.algoSelM 72 | % 3. Perform each algorithm 73 | tic; 74 | [pose, valid] = feval(config.algorithm{m,config.algoEstm}, obsData, map); 75 | elapse = toc * 1000; % [sec] to [msec] 76 | set.perf(1,1,m) = error_position(truth(1:3), pose(1:3)); 77 | set.perf(1,2,m) = tran_rad2deg(error_orientation(truth(4:6), pose(4:6))); % [rad] to [deg] 78 | projection = observe_bearing(map, pose); 79 | set.perf(1,3,m) = tran_rad2deg(norm(trim_rad(obsData - projection))); % [rad] to [deg] 80 | set.perf(1,4,m) = elapse; 81 | set.perf(1,5,m) = ~isequal(valid, config.algorithm{m,config.algoVald}); 82 | set.pose(1,:,m) = pose; 83 | end 84 | record.perf{1,1} = [record.perf{1,1}; set.perf]; 85 | record.pose{1,1} = [record.pose{1,1}; set.pose]; 86 | end 87 | disp([' * Experiment on dataset #', num2str(d), ' is complete.']); 88 | end 89 | 90 | % 4. Save experimental results 91 | save(config.matFile, 'config', 'criteria', 'record'); 92 | 93 | % To visualize the result, please use the script, 'run_draw_distribution', with 'target.ex = 1' and 'target.v = 1'. 94 | -------------------------------------------------------------------------------- /run_eval_mrclam.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Localization Evaluatioin (with MRCLAM Dataset) for Triangulation Toolbox =='); 5 | 6 | % Configure experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | config.dataFile = ... % A list of data files 8 | { ... 9 | 'dataset_mrclam/mrclam1.mat'; ... 10 | 'dataset_mrclam/mrclam2.mat'; ... 11 | 'dataset_mrclam/mrclam3.mat'; ... 12 | 'dataset_mrclam/mrclam4.mat'; ... 13 | 'dataset_mrclam/mrclam5.mat'; ... 14 | 'dataset_mrclam/mrclam6.mat'; ... 15 | 'dataset_mrclam/mrclam7.mat'; ... 16 | 'dataset_mrclam/mrclam8.mat'; ... 17 | 'dataset_mrclam/mrclam9.mat'; ... 18 | }; 19 | config.pose = []; 20 | config.algorithm = ... % Description of localization algorithms 21 | { ... 22 | % #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ... 23 | 1, 2, 'Sayed05-TOA2D',@localize2d_sayed05_toa, @observe_distance, 3, [1 1 0 0 0 0], 'kx:'; ... 24 | 2, 2, 'Sayed05-TDOA', @localize2d_sayed05_tdoa, @observe_distance_relative,3, [1 1 0 0 0 0], 'k--'; ... 25 | 3, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ... 26 | 4, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ... 27 | 5, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ... 28 | 6, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ... 29 | 7, 2, 'Sayed05-AOA', @localize2d_sayed05_aoa, @observe_displacement, 2, [1 1 0 0 0 1], 'ko-'; ... 30 | }; 31 | config.matFile = 'run_eval_mrclam.mat'; 32 | 33 | criteria.name = {'Position Error [m]', 'Orientation Error [deg]', ... 34 | 'Computing Time [msec]', 'Number of Failures'}; % Name of evaluation criteria 35 | criteria.repr = {@median, @median, @median, @sum}; % Functions for calculating representive values 36 | % (e.g. mean, median, std, and sum) 37 | criteria.format = {'%.6f', '%.3f', '%.6f', '%d'}; % Format for printing text 38 | 39 | % Perform experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 40 | config.algoDims = 2; 41 | config.algoName = 3; 42 | config.algoEstm = 4; 43 | config.algoObsv = 5; 44 | config.algoMinN = 6; 45 | config.algoVald = 7; 46 | config.algoLine = 8; 47 | config.algoSelM = 1:size(config.algorithm,1); 48 | 49 | for m = 1:size(config.algorithm,1) 50 | obsFuncName{m} = func2str(config.algorithm{m,config.algoObsv}); 51 | end 52 | record.perf{1,1} = []; 53 | record.pose{1,1} = []; 54 | for d = 1:size(config.dataFile,1) 55 | % 1. Read each data file which contains landmarks, ground truths, and measurements 56 | data = load(config.dataFile{d}); 57 | for t = 1:size(data.groundtruth,1) 58 | truth = [data.groundtruth(t,2:3), zeros(1,3), data.groundtruth(t,4)]; 59 | measure = [data.measurement(data.measurement(:,1) == t,2:end)]; 60 | map = [data.landmark(measure(:,1),2:end), zeros(size(measure,1),4)]; 61 | set.perf = zeros(1,4,size(config.algorithm,1)); 62 | set.pose = zeros(1,6,size(config.algorithm,1)); 63 | for m = config.algoSelM 64 | % 2. Rearrange measurements for each algorithm 65 | if isequal(obsFuncName{m}, 'observe_distance') 66 | obsData = measure(:,2); 67 | elseif isequal(obsFuncName{m}, 'observe_distance_relative') 68 | obsData = measure(:,2); 69 | obsData(2:end) = obsData(2:end) - obsData(1); 70 | elseif isequal(obsFuncName{m}, 'observe_bearing') 71 | obsData = [measure(:,3), zeros(size(measure,1),1)]; 72 | elseif isequal(obsFuncName{m}, 'observe_displacement') 73 | obsData = repmat(measure(:,2),1,3) .* ... 74 | [cos(measure(:,3)), sin(measure(:,3)), zeros(size(measure,1),1)]; 75 | else 76 | error(['This dataset cannot be applied to the algorithm #', num2str(m), '!']); 77 | end 78 | 79 | % 3. Perform each algorithm 80 | tic; 81 | [pose, valid] = feval(config.algorithm{m,config.algoEstm}, obsData, map); 82 | elapse = toc * 1000; % [sec] to [msec] 83 | set.perf(1,1,m) = error_position(truth(1:3), pose(1:3)); 84 | set.perf(1,2,m) = tran_rad2deg(error_orientation(truth(4:6), pose(4:6))); % [rad] to [deg] 85 | set.perf(1,3,m) = elapse; 86 | set.perf(1,4,m) = ~isequal(valid, config.algorithm{m,config.algoVald}); 87 | set.pose(1,:,m) = pose; 88 | end 89 | record.perf{1,1} = [record.perf{1,1}; set.perf]; 90 | record.pose{1,1} = [record.pose{1,1}; set.pose]; 91 | end 92 | disp([' * Experiment on dataset #', num2str(d), ' is complete.']); 93 | end 94 | 95 | % 4. Save experimental results 96 | save(config.matFile, 'config', 'criteria', 'record'); 97 | 98 | % To visualize the result, please use the script, 'run_draw_distribution', with 'target.ex = 1' and 'target.v = 1'. 99 | -------------------------------------------------------------------------------- /run_test_aux.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Unit-test of Auxiliary Functions for Triangulation Toolbox =='); 5 | 6 | % The given landmark map and true pose 7 | map = ... 8 | [ ... 9 | % x, y, z, r_x, r_y, r_z ... 10 | 0, 0, 0, 0, 0, 0; ... 11 | 5, 0, 5, 0, 0, tran_deg2rad( +90); ... 12 | 5, 5, 0, 0, 0, tran_deg2rad(-180); ... 13 | 0, 5, 5, 0, 0, tran_deg2rad( -90); ... 14 | ]; 15 | pose = [5, 5, 5, 0, 0, pi / 2]; 16 | 17 | % test_is_equal 18 | disp('==== test_is_true ===='); 19 | test_is_true(82 < 84); 20 | test_is_true([10, 18] == [10, 18]); 21 | test_is_true(~isequal([3, 2, 9], [1, 0, 1, 8])); 22 | 23 | % test_is_near 24 | disp('==== test_is_near ===='); 25 | test_is_near(82, 84, 5); 26 | test_is_near(4.17, 4.17 + eps); 27 | 28 | % error_position 29 | disp('==== error_position===='); 30 | test_is_true(error_position([1, 2], [3, 4]) == 2 * sqrt(2)); 31 | 32 | % error_orientation 33 | disp('==== error_orientation ===='); 34 | test_is_near(error_orientation([ 0, 0, 0], [ 0, 0, pi]), pi); 35 | test_is_near(error_orientation([ 0, 0, 0], [ 0, pi, 0]), pi); 36 | test_is_near(error_orientation([ 0, 0, 0], [pi, 0, 0]), 0); 37 | test_is_near(error_orientation([ 0, 0, 0], [pi, pi, 0]), pi); 38 | test_is_near(error_orientation([ 0, 0, 0], [ 0, pi, pi]), 0); 39 | 40 | % trim_rad 41 | disp('==== trim_rad ===='); 42 | test_is_true(trim_rad(pi) == -pi); 43 | test_is_true(trim_rad(-pi) == -pi); 44 | test_is_true(trim_rad(2 * pi) == 0); 45 | test_is_true(trim_rad(-2 * pi) == 0); 46 | 47 | in = (-4 * pi):(pi/100):(+4 * pi); 48 | out = trim_rad(in); 49 | figure('Color', [1, 1, 1]); 50 | hold on; 51 | plot(in, out); 52 | title('trim\_rad: Correctness on [-4 * pi, +4 * pi]'); 53 | xlabel('in [rad]'); 54 | ylabel('out [rad]'); 55 | axis equal; 56 | box on; 57 | grid on; 58 | hold off; 59 | 60 | % tran_rad2deg 61 | disp('==== tran_rad2deg ===='); 62 | test_is_near(tran_rad2deg(pi / 6), 30); 63 | test_is_near(tran_rad2deg(pi / 3), 60); 64 | 65 | % tran_deg2rad 66 | disp('==== tran_deg2rad ===='); 67 | test_is_near(tran_deg2rad(30), pi / 6); 68 | test_is_near(tran_deg2rad(60), pi / 3); 69 | 70 | % tran_rad2rot 71 | disp('==== tran_rad2rot ===='); 72 | test_is_near(tran_rad2rot([0, 0, pi / 2]), [0, -1, 0; 1, 0, 0; 0, 0, 1]); 73 | R = tran_rad2rot([pi / 6, pi / 6, pi / 6]); 74 | test_is_near(R * R', eye(3,3)); 75 | 76 | % tran_rot2rad 77 | disp('==== tran_rot2rad ===='); 78 | in = [pi / 6, pi / 4, pi / 3]; 79 | R = tran_rad2rot(in); 80 | out = tran_rot2rad(R); 81 | test_is_near(in, out); 82 | 83 | % observe_distance 84 | disp('==== observe_distance ===='); 85 | [obsData, obsMap] = observe_distance(map, pose); 86 | test_is_true(obsMap == map); 87 | test_is_near(obsData(1), norm([5, 5, 5])); 88 | test_is_near(obsData(2), norm([5, 0, 0])); 89 | test_is_near(obsData(3), norm([5, 0, 0])); 90 | test_is_near(obsData(4), norm([5, 0, 0])); 91 | 92 | test_is_true(isempty(observe_distance([], [0, 0, 0, 0, 0, 0]))); % In case of no landmark 93 | test_is_true(isempty(observe_distance([0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0))); % In case of zero visibility 94 | 95 | % observe_distance_relative 96 | disp('==== observe_distance_relative ===='); 97 | [obsData, obsMap] = observe_distance_relative(map, pose); 98 | test_is_true(obsMap == map); 99 | test_is_near(obsData(1), norm([5, 5, 5])); 100 | test_is_near(obsData(2), norm([5, 0, 0]) - norm([5, 5, 5])); 101 | test_is_near(obsData(3), norm([5, 0, 0]) - norm([5, 5, 5])); 102 | test_is_near(obsData(4), norm([5, 0, 0]) - norm([5, 5, 5])); 103 | 104 | % observe_displacement 105 | disp('==== observe_displacement ===='); 106 | [obsData, obsMap] = observe_displacement(map, pose); 107 | test_is_near(obsData(1,:), [-5, +5, -5]); 108 | test_is_near(obsData(2,:), [-5, 0, 0]); 109 | test_is_near(obsData(3,:), [ 0, 0, -5]); 110 | test_is_near(obsData(4,:), [ 0, 5, 0]); 111 | 112 | % observe_bearing 113 | disp('==== observe_bearing ===='); 114 | [obsData, obsMap] = observe_bearing(map, pose); 115 | test_is_near(obsData(1,:), [tran_deg2rad( 135), -acos(sqrt(2/3))]); 116 | test_is_near(obsData(2,:), [tran_deg2rad(-180), 0]); 117 | test_is_near(obsData(3,:), [tran_deg2rad( 0), -pi / 2]); 118 | test_is_near(obsData(4,:), [tran_deg2rad( 90), 0]); 119 | 120 | % observe_pose 121 | disp('==== observe_pose ===='); 122 | [obsData, obsMap] = observe_pose(map, pose); 123 | test_is_near(obsData(1,:), [-5, +5, -5, 0, 0, -pi / 2]); 124 | test_is_near(obsData(2,:), [-5, 0, 0, 0, 0, 0 ]); 125 | test_is_near(obsData(3,:), [ 0, 0, -5, 0, 0, +pi / 2]); 126 | test_is_near(obsData(4,:), [ 0, 5, 0, 0, 0, -pi ]); 127 | 128 | % apply_noise_gauss 129 | disp('==== apply_noise_gauss ===='); 130 | in = zeros(2000,2); 131 | out1 = apply_noise_gauss(in, 3); 132 | out2 = apply_noise_gauss(in, [1, 2]); 133 | out3 = apply_noise_gauss(in, [2, 1; 1, 2]); 134 | figure('Color', [1, 1, 1]); 135 | hold on; 136 | plot(out1(:,1), out1(:,2), 'r.'); 137 | plot(out2(:,1), out2(:,2), 'b.'); 138 | plot(out3(:,1), out3(:,2), 'g.'); 139 | title('apply\_noise\_gauss: Noise Distributions'); 140 | xlabel('Data X'); 141 | ylabel('Data Y'); 142 | legend({'\sigma = [3, 0; 0, 3]', '\sigma = [1, 0; 0, 2]', '\sigma = [2, 1; 1, 2]'}); 143 | grid on; 144 | axis equal; 145 | axis([-10, 10, -10, 10]); 146 | box on; 147 | hold off; 148 | -------------------------------------------------------------------------------- /dataset_roh/(X1.5,Y1.5).csv: -------------------------------------------------------------------------------- 1 | 132.9,-108.2,-45.4,16.2 2 | 132.6,-108.3,-45.3,16.8 3 | 132.6,-108.3,-45.4,17.2 4 | 132.9,-108.2,-45.3,17.8 5 | 132.4,-108.4,-45.4,17.9 6 | 132.7,-108.6,-45.5,17.8 7 | 132.8,-108.7,-45.5,17.8 8 | 132.3,-108.5,-45.6,17.7 9 | 131.9,-108.6,-45.6,17.7 10 | 132,-108.5,-45.7,17.9 11 | 132.5,-108,-45.7,18.2 12 | 132.6,-107.7,-45.8,18.5 13 | 133,-107.1,-45.8,19 14 | 133.1,-107,-45.8,18.7 15 | 133.2,-107.1,-45.7,18.7 16 | 133.2,-107.3,-45.6,18.5 17 | 132.9,-107.4,-45.4,18.1 18 | 132.6,-107.6,-45.4,17.8 19 | 132.1,-107.3,-45.2,17.8 20 | 132.2,-106.9,-45.1,17.7 21 | 132.7,-107.1,-45.3,17.7 22 | 133.1,-107.4,-45.4,17.9 23 | 133.1,-107.4,-45.7,18.3 24 | 133.1,-107.5,-45.7,17.9 25 | 132.7,-107.7,-46,18.4 26 | 132.4,-107.8,-46.1,18.4 27 | 132.3,-107.8,-45.9,18.4 28 | 132.3,-107.9,-45.8,18.3 29 | 132.2,-108,-45.6,18.2 30 | 132.2,-107.9,-45.4,18 31 | 132,-107.9,-45.3,17.9 32 | 131.9,-107.9,-45.1,18 33 | 132.1,-107.9,-45.2,17.8 34 | 132.1,-107.9,-45.2,17.8 35 | 132.2,-108,-45.5,17.5 36 | 132.8,-108.1,-45.6,17.2 37 | 132.7,-108.1,-45.7,17.4 38 | 132.8,-108.3,-45.8,17.4 39 | 133,-108.3,-45.6,17.5 40 | 133.2,-108.4,-45.7,17.7 41 | 133.5,-108.1,-45.7,17.5 42 | 134.1,-108,-45.7,17.3 43 | 134.4,-107.8,-45.6,17.3 44 | 134.3,-107.5,-45.6,17.3 45 | 134.1,-107.3,-45.6,17.3 46 | 134.3,-106.8,-45.6,17.3 47 | 133.4,-107.2,-45.4,17.9 48 | 132.9,-107.3,-45.5,18 49 | 133.2,-107.8,-45.6,18.2 50 | 132.5,-108.2,-45.4,17.9 51 | 132.6,-108.3,-45.5,18 52 | 132.8,-108.3,-45.3,17.8 53 | 132.3,-108.3,-45.7,17.7 54 | 132.2,-108.3,-45.8,17.7 55 | 132.3,-108.2,-45.9,18 56 | 132.2,-108,-46.3,18.2 57 | 132.3,-108.1,-46.2,18.5 58 | 133,-108.2,-46,18.2 59 | 133.5,-108,-46,17.3 60 | 134,-108,-45.6,16.4 61 | 134.8,-107.9,-45.4,16.1 62 | 135.7,-108.1,-45.2,16 63 | 135.1,-108.4,-44.9,16 64 | 135.1,-108,-45.1,16 65 | 134.6,-108.3,-44.9,16.8 66 | 134,-108,-45.2,17.2 67 | 133.4,-108.3,-45.4,17.4 68 | 132.9,-108.1,-45.5,17.2 69 | 132.6,-107.8,-45.9,17.3 70 | 132.8,-107.7,-46,17.1 71 | 132.8,-107.8,-46,17.3 72 | 132.8,-107.5,-45.8,17.7 73 | 132.6,-107.4,-45.6,17.9 74 | 132.4,-107.4,-45.4,17.7 75 | 132.4,-107.4,-45.2,17.6 76 | 132.3,-107.4,-45.1,17.4 77 | 132.1,-107.5,-45.1,17 78 | 132.1,-107.8,-44.9,17.1 79 | 131.7,-107.9,-44.8,17.3 80 | 131.4,-107.5,-44.8,17.3 81 | 131.3,-107.3,-45,17.4 82 | 131.2,-107.3,-45,17.4 83 | 131.2,-107.4,-45,17.4 84 | 131,-107.7,-45.2,17.4 85 | 132.4,-108.1,-45.4,17.7 86 | 133,-108.6,-45.3,17.8 87 | 133.3,-108.4,-45.1,17.7 88 | 132.9,-108.4,-45,17.5 89 | 132.8,-108.2,-44.9,17.3 90 | 132.9,-108.2,-45,17.3 91 | 133.2,-108.4,-45.3,17.6 92 | 133.4,-108.2,-45.4,17.7 93 | 133.7,-108.2,-45.4,17.9 94 | 133.4,-108.2,-45.5,18 95 | 132.5,-108.3,-45.2,18.2 96 | 132.3,-108.5,-45.2,18.2 97 | 132.2,-108.7,-44.8,17.9 98 | 132.5,-108.8,-44.7,17.8 99 | 133.1,-108.4,-44.5,17.1 100 | 133.2,-108.5,-44.6,17 101 | 133.9,-108.7,-44.9,17 102 | 133.6,-108.1,-45.1,16.5 103 | 133.3,-108.3,-45.5,16.7 104 | 132.5,-108,-45.9,16.6 105 | 132.5,-107.9,-45.9,16.9 106 | 132.4,-107.8,-45.8,17.3 107 | 138.3,-109.2,-45.3,17.2 108 | 137.7,-109,-45.4,17 109 | 136,-108.9,-45.6,16.9 110 | 135,-109,-45.6,17.1 111 | 134.1,-108.9,-45.7,17 112 | 132.8,-108.4,-45.8,17.3 113 | 132.6,-108.1,-45.8,17.5 114 | 132.8,-108,-45.6,17.6 115 | 132.9,-107.7,-45.4,17.8 116 | 132.5,-107.7,-45.3,17.8 117 | 132.4,-107.8,-45.1,17.5 118 | 132.5,-107.8,-44.9,17.4 119 | 132.4,-107.7,-44.7,17.2 120 | 132.3,-107.8,-44.5,17.1 121 | 132,-107.8,-44.6,16.9 122 | 132.2,-107.7,-44.8,17 123 | 132.1,-107.8,-44.9,17.1 124 | 132,-107.8,-45.1,17.4 125 | 132.6,-108,-45.3,17.7 126 | 133.3,-107.8,-45.5,17.9 127 | 133.7,-107.7,-45.4,17.9 128 | 133.8,-107.7,-45.3,17.9 129 | 133.5,-107.7,-45.1,17.8 130 | 133.3,-107.9,-45,17.5 131 | 132.8,-108,-45,17.4 132 | 132.7,-107.8,-45.1,17.1 133 | 132.7,-107.8,-45.2,17.1 134 | 133.1,-107.8,-45.3,17.5 135 | 133.4,-108,-45.3,17.8 136 | 133.3,-108,-45.6,18.3 137 | 133.2,-108,-45.6,18.3 138 | 132.9,-107.5,-45.6,18.7 139 | 132.7,-107.5,-45.6,18.8 140 | 132.4,-107.3,-45.6,19 141 | 132.3,-107.3,-45.7,18.8 142 | 132.5,-107.3,-45.5,18.7 143 | 131.4,-108.1,-45.5,18.4 144 | 131.9,-108,-45.5,18.1 145 | 132.6,-108.3,-45.5,17.8 146 | 133,-108.4,-45.5,17.5 147 | 133,-108.6,-45.5,17.2 148 | 133.9,-108.6,-45.5,17 149 | 133.8,-108.1,-45.3,17 150 | 133.3,-108.4,-45.5,16.8 151 | 132.6,-108.4,-45.7,16.7 152 | 132.3,-108.4,-45.7,16.7 153 | 132.1,-108.4,-45.6,16.8 154 | 131.8,-108.3,-45.6,17.2 155 | 132,-108.4,-45.7,17.2 156 | 132.4,-108,-45.7,17.9 157 | 132.8,-107.9,-45.5,18 158 | 132.7,-108,-45.4,18 159 | 132.7,-107.9,-45.7,18.2 160 | 132.8,-108.1,-45.9,18.2 161 | 132.4,-108.1,-45.6,18.2 162 | 132.4,-108,-45.4,18 163 | 132.5,-108,-45.3,18 164 | 132.5,-108,-45.4,18 165 | 132.1,-107.8,-45.6,17.9 166 | 132,-107.7,-45.7,17.9 167 | 131.9,-107.7,-46.1,17.5 168 | 132.6,-107.4,-46.2,17.1 169 | 133.4,-107.5,-46.3,16.8 170 | 134.3,-107.7,-46.5,16.4 171 | 134.1,-107.9,-46,16.4 172 | 134,-107.8,-45.6,16.6 173 | 134.1,-108,-45.5,17.2 174 | 133.3,-108.3,-45.5,17.4 175 | 132.4,-108.2,-45.6,17.4 176 | 131.7,-108.1,-45.4,17.7 177 | 132,-108.1,-45.5,17.7 178 | 132.1,-108.3,-45.3,17.3 179 | 132.8,-107.9,-45.3,17.8 180 | 132.9,-107.9,-45.5,18 181 | 133.2,-107.8,-45,18.1 182 | 133,-107.9,-44.9,17.9 183 | 132.9,-107.9,-45.3,17.7 184 | 132.8,-107.9,-45.6,17.7 185 | 132.9,-108,-45.5,17.5 186 | 132.9,-107.8,-45.2,17.8 187 | 132.9,-107.7,-45.5,17.6 188 | 133.2,-107.8,-45.5,17.9 189 | 132.1,-105.7,-45,18 190 | 132.7,-105.6,-45.1,18.1 191 | 132.5,-106.1,-45.1,17.7 192 | 133.6,-108.4,-45.2,17.5 193 | 133,-108.3,-45.4,17.6 194 | 132.8,-108.4,-45.6,17.9 195 | 132.8,-108.4,-46.1,17.9 196 | 132.8,-108,-46.3,17.9 197 | 132.8,-108.2,-46.4,18.1 198 | 133.3,-107.9,-46.2,17.9 199 | 133.2,-107.8,-46.1,17.9 200 | 133.2,-108,-45.6,17.5 201 | -------------------------------------------------------------------------------- /dataset_roh/(X3.0,Y1.5).csv: -------------------------------------------------------------------------------- 1 | 117.5,-112.8,-32.5,35.4 2 | 117.1,-113.2,-31.9,36 3 | 117.1,-112.7,-31,36.4 4 | 116.8,-112.5,-30.8,36.4 5 | 116.7,-112.4,-30.5,36.5 6 | 116.7,-112.1,-30.2,36.8 7 | 116.6,-112,-30.1,36.9 8 | 116.4,-112,-30,37.2 9 | 116.3,-112.1,-29.9,37.4 10 | 116.5,-112.1,-30,37.4 11 | 116.6,-112.2,-30.4,37 12 | 116.8,-112.3,-30.6,36.9 13 | 117.2,-112.4,-30.8,36.7 14 | 117.6,-112.6,-31,36.7 15 | 117.4,-112.8,-31.1,36.4 16 | 117.7,-112.7,-31.1,36.3 17 | 117.8,-112.8,-31.2,36.5 18 | 117.6,-112.6,-31.2,36.6 19 | 117.4,-112.6,-31.3,36.7 20 | 117.4,-112.5,-31.4,36.7 21 | 117.2,-112.3,-31.4,36.6 22 | 117.5,-112.4,-31.5,36.6 23 | 117.5,-112.2,-31.6,36.2 24 | 117.9,-111.6,-31.5,36.2 25 | 118,-111.2,-31.5,36.1 26 | 118.2,-110.8,-31.6,36.1 27 | 118.5,-111,-31.3,36.2 28 | 118.4,-110.6,-31.1,36.3 29 | 118.7,-110.5,-30.8,36.6 30 | 118.1,-110.8,-30.7,36.5 31 | 118.3,-111,-30.5,36.4 32 | 118.1,-111.5,-30.4,36.4 33 | 118.4,-111.4,-30.4,36.6 34 | 118.1,-112.1,-30.6,37 35 | 118.6,-112,-30.6,37.3 36 | 118.4,-111.9,-30.8,37.6 37 | 118.3,-111.5,-31,37.8 38 | 118.1,-111.4,-30.9,37.8 39 | 117.9,-111.2,-31,37.6 40 | 117.9,-110.9,-30.9,37.6 41 | 117.5,-111.2,-30.9,37.5 42 | 117.3,-111.5,-30.8,37.1 43 | 117,-111.9,-31.2,36.7 44 | 117.3,-111,-31.2,36.4 45 | 117.5,-110.2,-31.3,36.3 46 | 117.2,-108.9,-31.4,36 47 | 117.3,-108.1,-31.5,35.6 48 | 116.5,-107.7,-31.6,35.7 49 | 116.6,-107.5,-31,35.7 50 | 116.3,-107,-30.8,35.7 51 | 116.1,-107.7,-30.4,35.8 52 | 116.6,-107.9,-30.5,36 53 | 116.7,-108.7,-30.3,36.4 54 | 117.9,-108.9,-30.2,36.6 55 | 118.1,-108.9,-30.3,37 56 | 118.3,-109.9,-30.2,37 57 | 118.1,-111.5,-30.4,37.1 58 | 117.9,-111.8,-30.4,36.9 59 | 117.3,-112.5,-30.6,37 60 | 117.5,-112.7,-30.9,36.8 61 | 117.5,-112.4,-30.9,37.1 62 | 117.5,-112.1,-30.9,37 63 | 117.5,-111.9,-30.9,36.7 64 | 118,-112.1,-31.1,36.6 65 | 117.7,-111.6,-31.2,36.4 66 | 118,-111.6,-31.2,36.2 67 | 118,-111.1,-31.4,36.4 68 | 118.3,-110.9,-31.1,36.8 69 | 118.6,-110.9,-30.7,37 70 | 118,-111.1,-30.6,37 71 | 118,-111.5,-30.6,36.9 72 | 117.6,-111.7,-30.4,36.7 73 | 117.5,-112,-30.2,36.6 74 | 117,-112.1,-30.4,36.3 75 | 116.9,-111.8,-30.5,36.1 76 | 117.4,-112.1,-30.5,36.2 77 | 117.7,-112.2,-30.1,36.4 78 | 118,-112.2,-29.9,36.6 79 | 118.2,-111.7,-30,36.5 80 | 118,-111.7,-30.6,36.4 81 | 117.5,-112,-31.4,36.5 82 | 117.1,-112.6,-31.7,36.5 83 | 117.4,-112.2,-31.9,36 84 | 117.8,-111,-31.7,35.8 85 | 118.2,-109.7,-31.5,35.9 86 | 118.2,-108.6,-31.4,35.9 87 | 118.2,-108,-31.2,35.8 88 | 117.9,-107.4,-31,35.6 89 | 117.8,-107.2,-31,35.8 90 | 117.5,-107.7,-30.9,35.9 91 | 117.2,-107.8,-30.9,36.1 92 | 117.4,-108.6,-30.9,36.2 93 | 117.4,-108.9,-30.9,36.4 94 | 117.4,-108.9,-30.9,36.9 95 | 117.2,-109.1,-30.7,37.3 96 | 117.1,-109.9,-30.7,37.7 97 | 117.1,-111,-30.6,37.9 98 | 116.9,-111.2,-30.8,37.7 99 | 117,-112.1,-31,37.6 100 | 117.1,-112.6,-31.2,37.5 101 | 117.3,-112.5,-31.2,37.4 102 | 117.6,-112.4,-30.9,37.4 103 | 117.9,-112.1,-30.6,37 104 | 118.1,-112.4,-30.5,36.4 105 | 118.1,-111.6,-30.4,36.2 106 | 118.2,-111.5,-30.5,35.9 107 | 118.2,-111.1,-30.4,35.7 108 | 118.4,-110.8,-30.5,36.1 109 | 118.1,-110.7,-30.5,36.4 110 | 118.1,-110.6,-30.4,36.4 111 | 117.9,-111.4,-30.6,36.6 112 | 117.5,-112.1,-30.7,36.5 113 | 117.5,-112.4,-30.7,36.3 114 | 117.4,-112.1,-30.8,36.1 115 | 117.6,-111.6,-30.7,36.1 116 | 117.5,-111.8,-30.5,36.2 117 | 117.5,-111.7,-30.3,36.4 118 | 117.8,-111.7,-30.4,36.7 119 | 117.3,-111.9,-31.2,36.7 120 | 117.4,-112.3,-31.7,36.7 121 | 116.9,-112.9,-31.8,36.7 122 | 117.2,-112.8,-31.9,36.5 123 | 117.3,-112,-32.1,36.6 124 | 117.2,-111.6,-31.9,36.7 125 | 117.4,-111.5,-31.6,37 126 | 117.7,-111.4,-31.2,37 127 | 117.6,-111.2,-30.9,36.9 128 | 117.3,-111.5,-30.6,36.9 129 | 117.3,-112,-30.4,36.7 130 | 117.5,-112.1,-30.4,36.6 131 | 117.3,-111.9,-30.5,36.5 132 | 117.4,-112.1,-30.7,36.4 133 | 117.7,-111.8,-31.5,36.4 134 | 117.8,-112.1,-31.6,36.6 135 | 117.4,-112.2,-31.8,36.5 136 | 117.2,-112.6,-31.7,36.5 137 | 117.1,-112.8,-31.5,36.6 138 | 117.4,-112.5,-31.7,36.9 139 | 117.4,-112.2,-31.2,36.9 140 | 117.4,-112,-30.8,36.8 141 | 117.3,-111.8,-30.6,36.6 142 | 117.1,-111.4,-30.5,36.4 143 | 117.1,-111.5,-30.5,36.3 144 | 117.2,-111.2,-30.4,36.4 145 | 117.8,-111,-30.4,36.4 146 | 117.8,-111,-30.4,36.6 147 | 118,-111.2,-30.5,36.7 148 | 118,-111.6,-30.7,36.9 149 | 117.5,-112.1,-31,36.9 150 | 116.8,-112.2,-31.2,37.1 151 | 117.1,-112.2,-31.1,37.1 152 | 117.6,-111.7,-30.9,37.3 153 | 118.1,-111.4,-30.6,37 154 | 118.3,-111.7,-30.4,36.9 155 | 118.3,-112.1,-30.3,36.8 156 | 118,-112.9,-30.4,37 157 | 117.9,-113,-31.3,36.6 158 | 116.9,-112.9,-31.4,36.2 159 | 116.8,-112.1,-31.4,36.3 160 | 116.4,-111.8,-31.4,36.1 161 | 116.2,-111.9,-31.1,36.2 162 | 116.6,-111.6,-30.8,36.5 163 | 116.6,-111.4,-30.8,36.5 164 | 116.9,-111.6,-31,36.8 165 | 116.9,-111.9,-30.8,36.6 166 | 117.1,-112,-30.7,36.8 167 | 117.2,-111.7,-30.5,36.6 168 | 116.9,-111.6,-30.2,36.5 169 | 117.2,-111.3,-29.7,36.7 170 | 117.5,-111.3,-29.6,36.6 171 | 117.8,-111.7,-30.4,36.5 172 | 117.9,-112,-30.8,36.5 173 | 117.5,-112.6,-31.6,36.4 174 | 117.5,-112.4,-32,36.3 175 | 117.3,-112.6,-31.6,36.6 176 | 117.5,-112.3,-31.1,36.9 177 | 117,-112.7,-31,36.7 178 | 117.5,-112,-30.8,36.5 179 | 117.4,-111.6,-30.8,36.3 180 | 117.3,-111.3,-30.7,36.1 181 | 117.1,-110.8,-30.9,35.9 182 | 117.2,-110.5,-31,35.9 183 | 117.6,-109.7,-30.8,36.4 184 | 117,-110,-30.8,36.9 185 | 117,-110.3,-30.7,37.1 186 | 116.9,-112.2,-30.8,37.3 187 | 117.3,-112.8,-30.8,37.2 188 | 117.7,-112.7,-30.9,37.1 189 | 117.7,-112.3,-30.9,37.5 190 | 117.7,-112,-30.5,37.4 191 | 117.6,-112,-30.1,37.4 192 | 117.9,-111.9,-29.9,37.1 193 | 118,-112,-29.7,36.7 194 | 118,-111.3,-30,36.2 195 | 118,-110.8,-30.3,36 196 | 118,-110.3,-30.5,36 197 | 117.6,-110,-30.7,36.3 198 | 117.3,-109.7,-31,36.4 199 | 116.9,-109.4,-31,36.5 200 | 117,-110,-30.8,36.3 201 | -------------------------------------------------------------------------------- /dataset_roh/(X1.5,Y4.5).csv: -------------------------------------------------------------------------------- 1 | 164.1,-131.1,-69,54.8 2 | 164,-130.7,-68.2,55.7 3 | 164.2,-130.6,-67.7,54.9 4 | 163.9,-130.5,-67.5,57 5 | 164,-130.2,-67.1,56.2 6 | 164.1,-130,-66.9,55.6 7 | 164.2,-129.8,-66.8,56 8 | 164.5,-129.9,-66.8,55.9 9 | 164.7,-129.7,-66.9,57.6 10 | 164.8,-129.8,-66.9,54.9 11 | 164.8,-129.8,-66.9,54.7 12 | 164.9,-129.8,-66.9,54.5 13 | 165,-129.9,-66.8,52.9 14 | 165,-129.9,-66.9,50.7 15 | 164.9,-130,-66.6,49 16 | 165,-130,-66.4,48.7 17 | 165.3,-130.1,-66.3,48.9 18 | 165.2,-130.1,-66.4,49.2 19 | 165.2,-130.2,-66.6,49.8 20 | 165.1,-130.3,-66.7,49.9 21 | 165.1,-130.3,-66.7,50.2 22 | 164.9,-130.3,-66.6,51.7 23 | 164.6,-130.4,-66.4,52.6 24 | 164.5,-130.4,-66.4,53.3 25 | 164.4,-130.4,-66.5,55.2 26 | 164.3,-130.4,-66.7,55.4 27 | 164.2,-130.5,-67,57.3 28 | 164.1,-130.5,-67.2,58.6 29 | 164.4,-130.2,-67.3,60.7 30 | 164.5,-130.1,-67.3,62.2 31 | 164.8,-130.1,-67.3,62.9 32 | 165,-130.3,-67.4,65.1 33 | 165.3,-130.3,-67.1,63.1 34 | 165.5,-130.4,-67.1,60.4 35 | 165.8,-130.8,-67.1,57.6 36 | 165.9,-131.1,-67.1,55.5 37 | 165.8,-131.2,-66.9,52.6 38 | 165.8,-131.1,-66.8,50 39 | 165.7,-131.1,-66.8,49.9 40 | 165.7,-131,-66.8,49.9 41 | 165.7,-131.1,-66.7,50.1 42 | 165.7,-131.1,-66.7,49.8 43 | 165.6,-131.1,-66.9,50.1 44 | 165.6,-131,-67,50 45 | 165.5,-131,-67,50.3 46 | 165.5,-131.2,-67.1,50.6 47 | 165.4,-131.2,-67.1,50.3 48 | 165.4,-131,-67,50.5 49 | 165.4,-131,-66.9,50.5 50 | 165,-131.1,-67.2,51.6 51 | 164.7,-131.2,-67.9,51.5 52 | 164.7,-131,-68.3,51.4 53 | 164.6,-130.8,-68.4,51.3 54 | 164.6,-130.9,-68.4,51.5 55 | 164.5,-130.6,-68.5,53 56 | 164.5,-130.6,-68.5,53 57 | 164.5,-130.6,-68.5,53 58 | 165.2,-130.4,-67.3,51.1 59 | 165.1,-130.6,-67.5,51.3 60 | 165.1,-130.7,-67.7,51.1 61 | 165.3,-130.8,-67.8,51.3 62 | 165.3,-131,-68,52.1 63 | 165.3,-131.1,-67.8,52.3 64 | 165.3,-131,-67.8,52.5 65 | 165.5,-130.8,-67.7,52.3 66 | 165.6,-130.6,-67.5,51.9 67 | 165.5,-130.8,-67.3,49.9 68 | 165.5,-130.6,-67.2,49.4 69 | 165.5,-130.8,-67.2,48.9 70 | 165.4,-130.9,-67,48.8 71 | 165.1,-131.1,-66.9,48.6 72 | 164.9,-131.2,-66.8,49.1 73 | 164.9,-131,-66.6,49.5 74 | 164.8,-131.1,-66.4,49.7 75 | 164.6,-131,-66.3,50 76 | 164.8,-130.8,-66.4,50.1 77 | 164.5,-130.8,-67,50.4 78 | 164.4,-130.8,-67.6,50.4 79 | 164.4,-130.8,-68,50.5 80 | 164.4,-130.7,-68,50.8 81 | 164.4,-131,-68.6,50.8 82 | 164.4,-131,-68.7,51.3 83 | 164.6,-131.1,-68.3,52.6 84 | 164.4,-131.2,-68,54.1 85 | 164.3,-131.4,-67.8,57.1 86 | 164.5,-131.5,-67.8,59.7 87 | 164.4,-131.2,-67.5,59.7 88 | 164.4,-131.3,-67.3,62.1 89 | 164.4,-130.9,-67.3,62.6 90 | 164.6,-130.7,-67.3,62.9 91 | 164.8,-130.6,-67,62 92 | 164.6,-130.4,-67.1,61.9 93 | 164.7,-130.4,-67.1,63.7 94 | 164.7,-130.2,-67.4,62.9 95 | 164.8,-130.6,-67.4,62 96 | 165,-130.7,-67.6,62.1 97 | 165.1,-130.6,-68,59.9 98 | 165.1,-130.7,-68.2,57.1 99 | 165.1,-131,-68.2,54.7 100 | 165.3,-131,-68.3,53.2 101 | 165.2,-130.8,-68.2,51 102 | 165.3,-131,-68,52.8 103 | 165.1,-130.7,-68.3,53.2 104 | 165.1,-130.4,-68.6,52 105 | 165.1,-130.4,-68.9,52.4 106 | 165.1,-130.1,-69.2,52.7 107 | 165,-130.1,-69.2,53.2 108 | 164.8,-130.1,-69.2,52.2 109 | 165,-130.2,-68.7,53.4 110 | 165.1,-130.3,-68.2,54.2 111 | 165.1,-130.2,-67.8,53.3 112 | 164.9,-130.3,-67.6,54.6 113 | 165.1,-130.1,-67.7,54.1 114 | 165.1,-130.3,-67.9,53.6 115 | 165.1,-130.4,-68,51.7 116 | 165.1,-130.5,-68.2,49.8 117 | 164.9,-130.7,-68.3,49.9 118 | 164.9,-130.8,-68.2,49.8 119 | 164.8,-131,-68,49.6 120 | 164.5,-130.8,-67.9,49.2 121 | 164.5,-130.7,-67.7,49.8 122 | 164.5,-130.5,-67.5,49.9 123 | 164.7,-130.4,-67.4,50.1 124 | 164.7,-130.2,-67.2,50.2 125 | 164.7,-130,-67.1,50.4 126 | 165.2,-129.9,-66.9,50.5 127 | 165.2,-129.7,-66.9,50.3 128 | 165,-129.6,-66.8,50.4 129 | 164.9,-129.4,-67,50.9 130 | 164.8,-129.6,-67.1,49.2 131 | 165.2,-129.6,-67.1,50 132 | 164.6,-129.9,-67.1,50.3 133 | 164.5,-130.5,-67.3,51.5 134 | 164.6,-130.7,-67.9,51.2 135 | 164.5,-130.6,-68,52.4 136 | 164.1,-130.7,-68,53.7 137 | 164.5,-130.6,-68.1,54.3 138 | 164.5,-130.4,-68.2,56.4 139 | 164.5,-130.3,-67.9,59 140 | 164.5,-130.1,-67.4,61.8 141 | 164.5,-130.1,-67.1,63.5 142 | 164.4,-129.9,-67,63.5 143 | 164.2,-130,-66.8,64.7 144 | 164.3,-130.2,-66.7,63.9 145 | 164.3,-130.6,-66.5,63.6 146 | 164.3,-131.1,-66.7,62.1 147 | 164.3,-131.5,-66.8,59.3 148 | 164.4,-131.6,-66.9,57.3 149 | 164.6,-131.8,-67.2,55.4 150 | 164.8,-131.9,-67.3,53.4 151 | 164.8,-131.8,-67.5,51 152 | 164.8,-131.3,-67.3,49.7 153 | 164.8,-130.8,-67.3,49.5 154 | 164.8,-130.9,-67.5,49.1 155 | 164.7,-130.9,-68.4,53.1 156 | 164.4,-130.5,-69,53.1 157 | 164.2,-130.4,-69.8,53.4 158 | 164.1,-130.7,-70.3,53.6 159 | 164.2,-131.1,-70.6,54.1 160 | 164.1,-131.1,-70.9,51.7 161 | 164.2,-130.9,-70.1,51.4 162 | 164.5,-131,-69.7,53 163 | 164.7,-130.9,-68.9,53.6 164 | 164.9,-130.7,-68.3,53.6 165 | 164.7,-130.6,-68,52.9 166 | 164.9,-130.4,-67.8,51.6 167 | 165.1,-130.6,-68,51.3 168 | 165.2,-130.7,-68.2,49.3 169 | 165.4,-130.9,-68.2,48.9 170 | 165.3,-131,-68.3,50.2 171 | 165.3,-131.1,-68.3,50.4 172 | 165.3,-131.3,-68.1,50.6 173 | 165.2,-131.1,-67.9,52.1 174 | 165,-131,-67.8,53.7 175 | 165.2,-130.8,-67.8,53.1 176 | 165.1,-130.6,-67.6,51.6 177 | 165.2,-130.5,-67.5,52.1 178 | 165.2,-130.3,-67.3,52.2 179 | 165.2,-130.2,-67.2,50.7 180 | 165.2,-130,-67.1,49.4 181 | 164.9,-129.9,-67.2,49.5 182 | 164.9,-129.7,-67,49.4 183 | 164.6,-130,-67.6,49.9 184 | 164.4,-130.3,-68.1,50.2 185 | 164.3,-130.6,-68.8,50.5 186 | 164.1,-130.8,-69.2,50.5 187 | 164.1,-130.8,-69.1,51.2 188 | 163.8,-130.8,-69.3,51.5 189 | 163.9,-130.6,-68.8,51.2 190 | 164,-130.5,-68.5,53.1 191 | 164,-130.5,-67.9,52.8 192 | 163.9,-130.3,-67.6,54 193 | 164.1,-130.5,-67.5,54.9 194 | 164.2,-130.6,-67.5,54.9 195 | 164.5,-130.5,-67.3,54.9 196 | 164.5,-130.6,-67.1,55.5 197 | 164.7,-130.3,-67.1,58.2 198 | 165,-130.4,-67.1,59.7 199 | 164.8,-130.3,-67,60.3 200 | 164.6,-130.1,-66.8,62.7 201 | -------------------------------------------------------------------------------- /dataset_roh/(X1.5,Y3.0).csv: -------------------------------------------------------------------------------- 1 | 152,-122.5,-57.6,25.1 2 | 152.4,-123.1,-57.3,24.8 3 | 152.4,-123.4,-57,24.8 4 | 151.7,-123.4,-56.8,25.1 5 | 151.8,-123.6,-56.8,25.1 6 | 151.8,-123.6,-56.8,25.5 7 | 152,-123.4,-56.9,25.8 8 | 153.5,-118.5,-55.5,26.3 9 | 152,-121.8,-57.4,25.1 10 | 152,-123.5,-57.2,24.6 11 | 152.3,-123.6,-56.9,24.6 12 | 152.4,-123.6,-56.7,24.8 13 | 155,-118.7,-53.6,26.7 14 | 151.6,-123.8,-58.1,26.6 15 | 151.7,-123.3,-57.2,25.5 16 | 151.9,-123.5,-56.7,25.1 17 | 152.3,-123.9,-56.5,24.5 18 | 152,-124,-56.6,24.7 19 | 152,-124.2,-56.8,25.1 20 | 150.9,-124,-56.9,25.3 21 | 152.2,-122.2,-57.9,26.2 22 | 151.3,-123.4,-56.8,24.8 23 | 151.5,-123.8,-56.6,24.3 24 | 151.5,-124.1,-56.6,24.5 25 | 151.2,-123.9,-56.5,25 26 | 156,-119.3,-56,26.3 27 | 151.3,-120.9,-56.7,26.1 28 | 151.3,-120.9,-56.7,26.1 29 | 151.9,-123.6,-56.4,25.4 30 | 151.9,-123.6,-56.2,25.4 31 | 151.9,-123.6,-56.2,25.7 32 | 152.2,-124.1,-56.4,26.1 33 | 152.2,-123.9,-56.5,26.3 34 | 152.2,-123.6,-56.6,26.6 35 | 152.1,-123.4,-56.9,27 36 | 152.1,-123.4,-56.9,26.4 37 | 152.1,-123.5,-56.9,25.8 38 | 152,-123.6,-57,25.2 39 | 151.8,-123.7,-57,24.8 40 | 151.6,-123.8,-56.8,24.2 41 | 151.6,-124.2,-56.6,23.8 42 | 151.5,-124.3,-56.6,24.2 43 | 151.4,-124.1,-56.6,24.4 44 | 151.3,-123.9,-56.4,24.9 45 | 151.2,-123.8,-56.6,25.3 46 | 151.4,-123.6,-56.7,25.8 47 | 151.4,-123.1,-56.8,26.1 48 | 154,-118.8,-56.8,23.7 49 | 151.8,-124.4,-58.2,27.7 50 | 152.6,-124.3,-57.8,27.5 51 | 153,-124,-57.3,26.8 52 | 153.2,-123.8,-57.2,25.8 53 | 152.9,-123.8,-57,25.4 54 | 153.2,-123.6,-56.8,24.5 55 | 153.2,-123.6,-56.8,23.8 56 | 151,-123.1,-56.8,23.8 57 | 151.2,-120.9,-57.2,24.8 58 | 150.8,-121.8,-56.9,24.4 59 | 151.1,-122.7,-57,23.9 60 | 151.2,-123.2,-56.7,24.2 61 | 151.3,-123.1,-56.4,24.6 62 | 151.5,-123.1,-56.6,24.9 63 | 151.3,-123.1,-56.8,25.6 64 | 151.5,-123.4,-56.7,25.7 65 | 152.1,-123.8,-56.7,26.4 66 | 152,-124.3,-56.7,25.4 67 | 152,-124.3,-56.7,25.4 68 | 152.7,-124.5,-56.8,24.3 69 | 152.4,-124.3,-56.7,23.6 70 | 152.3,-124,-56.9,22.7 71 | 152.3,-124,-56.9,22.7 72 | 151.3,-123.6,-56.9,23.5 73 | 151.7,-123.5,-56.8,24.3 74 | 152,-123.5,-56.8,25 75 | 151.9,-123.7,-57,25.7 76 | 151.7,-123.8,-57,25.7 77 | 151.5,-123.7,-57.1,25.6 78 | 151.5,-123.7,-57.3,25.4 79 | 151.4,-123.4,-57.1,25.4 80 | 151.4,-123.5,-57,25.3 81 | 151.6,-123.2,-57.1,25.7 82 | 151.8,-123.3,-57,25.8 83 | 151.9,-123.3,-56.7,26 84 | 151.2,-123.4,-56.4,26.3 85 | 150.9,-123.5,-56.7,26 86 | 151.2,-123.1,-57,25.5 87 | 152,-123.1,-56.9,25.3 88 | 152,-123.1,-56.9,25.3 89 | 151.6,-123.2,-57,24.2 90 | 151.8,-123,-57.3,23.8 91 | 152.1,-122.7,-57,24.4 92 | 151.8,-123.4,-56.8,25 93 | 150.9,-123.6,-56.8,24.8 94 | 151.7,-123.6,-56.9,25.9 95 | 151.9,-124.3,-56.7,25.9 96 | 151.8,-123.9,-56.9,25.8 97 | 152,-123.7,-56.9,26.1 98 | 155.4,-121,-56.9,26.2 99 | 152.3,-124.3,-58,26.5 100 | 152,-123.4,-57.8,25.8 101 | 151.9,-123.4,-56.8,25.3 102 | 151.8,-123.5,-56.6,24.9 103 | 151.7,-123.4,-56.9,24.8 104 | 151.7,-123.5,-57,25.1 105 | 151.9,-123.5,-57,25.3 106 | 151.7,-123.7,-57,25.3 107 | 151.7,-123.7,-57,25.3 108 | 151.6,-123.5,-57.1,25.3 109 | 151.8,-123.6,-56.9,26 110 | 151.9,-122.9,-56.7,26.8 111 | 151.9,-123,-56.8,26.8 112 | 151.5,-123.3,-57.2,26.5 113 | 151.2,-123.4,-57.1,25.8 114 | 150.9,-123.6,-57.2,25.2 115 | 150.4,-124,-57.3,24.8 116 | 150.4,-124,-57.4,24.5 117 | 150.4,-123.8,-57.4,24.3 118 | 150.8,-123.8,-57,23.9 119 | 151.1,-123.9,-57,24.2 120 | 151.3,-123.7,-56.9,24.9 121 | 151.7,-123.4,-56.8,24.9 122 | 151.8,-123.5,-56.9,25.1 123 | 151.6,-123.5,-57.1,25.1 124 | 151.8,-123.2,-57.3,25.3 125 | 151.7,-123.5,-57.1,25.6 126 | 151.7,-123.8,-57.1,25.6 127 | 151.7,-123.5,-57.2,25.7 128 | 151.5,-123.5,-57.2,25.7 129 | 151.5,-123.6,-57.1,25.7 130 | 151.5,-123.3,-56.8,25.9 131 | 151.6,-123.5,-57,25.8 132 | 150.9,-123.4,-56.9,25.1 133 | 150.4,-123.2,-56.9,24.3 134 | 150.1,-123.4,-56.9,23.9 135 | 150,-123.5,-56.8,23.6 136 | 150,-123.5,-56.8,23.3 137 | 150.4,-123.7,-56.9,24.1 138 | 151,-123.4,-56.9,24.3 139 | 151.7,-123,-56.6,25.3 140 | 151.8,-123,-56.4,25.6 141 | 151.6,-123,-56.4,25.4 142 | 151.6,-123.3,-56.4,25.6 143 | 151.6,-123.5,-56.3,25.6 144 | 151.5,-123.5,-56.5,25.7 145 | 151.6,-123.4,-56.6,26.3 146 | 152,-122.8,-56.9,26 147 | 151.7,-122.9,-56.9,26.7 148 | 151.1,-122.9,-56.8,26.3 149 | 150.7,-123,-57.1,25.8 150 | 150.4,-123,-57,25.4 151 | 149.9,-123.6,-56.7,25.1 152 | 149.3,-123.5,-56.6,24.5 153 | 149.9,-123.3,-56.6,23.5 154 | 150.3,-123,-56.5,23.5 155 | 150.9,-123,-56.4,24.1 156 | 151.2,-123.1,-56.4,24.4 157 | 151.3,-123.1,-56.7,24.6 158 | 151.3,-123.1,-56.8,24.9 159 | 151.3,-123.4,-56.5,25.2 160 | 151.2,-123.8,-56.5,25.4 161 | 151.2,-124.1,-56.5,25.4 162 | 151.2,-123.8,-56.5,25.4 163 | 151.2,-123.5,-56.5,25.5 164 | 151.6,-123.6,-56.8,25.6 165 | 151.8,-123.4,-56.9,25.6 166 | 152,-122.9,-56.9,25.7 167 | 151.8,-123.8,-57.2,26.8 168 | 151.6,-124,-57.6,26.9 169 | 151.6,-123.5,-57.9,26.9 170 | 152.6,-123.6,-58,27 171 | 151.9,-123.7,-58,26.2 172 | 151.8,-123.7,-58.1,25.6 173 | 152.1,-123.2,-57.7,25 174 | 151.9,-122.9,-57.3,24.3 175 | 151.7,-123.6,-56.9,24 176 | 150.5,-123.6,-56.7,23.3 177 | 151.3,-123.6,-56.7,23.9 178 | 151.5,-123.7,-56.6,24.2 179 | 151.1,-123.9,-56.5,24.1 180 | 151.4,-123.9,-56.3,24.7 181 | 151.5,-123.8,-56.5,25 182 | 151.5,-123.5,-56.8,25.2 183 | 151.5,-123.5,-56.6,25.2 184 | 151.8,-123.5,-56.4,25.2 185 | 151.9,-123.5,-56.4,25.4 186 | 151.8,-123.5,-56.5,25.4 187 | 152,-123.4,-56.2,25.5 188 | 152.3,-123.6,-56.2,25.7 189 | 152.1,-123.6,-56.2,25.7 190 | 152.1,-123.6,-56.2,25.8 191 | 152.3,-123.6,-56,25.9 192 | 151.8,-123.4,-56.7,26.2 193 | 151.6,-122.5,-57.3,26.7 194 | 151.5,-121.5,-57.4,26.9 195 | 151.1,-121.6,-57.4,25.9 196 | 150.6,-121.5,-57.6,25.3 197 | 150.2,-121.3,-57.6,24.6 198 | 150,-121.3,-57.3,23.9 199 | 149.9,-122.3,-57.1,23.3 200 | 151.1,-123.2,-57.3,23.2 201 | -------------------------------------------------------------------------------- /dataset_roh/(X4.5,Y3.0).csv: -------------------------------------------------------------------------------- 1 | 121.6,-152.7,-26.5,56.7 2 | 121.8,-153,-27,56.5 3 | 121.6,-153.7,-27.5,56.2 4 | 121.3,-154.1,-27.8,56.1 5 | 121.1,-153.9,-27.7,56 6 | 121.2,-153.5,-27.5,56 7 | 121.6,-153.2,-27.1,55.7 8 | 121.8,-153.3,-26.8,56 9 | 122.2,-153,-26.7,56.2 10 | 122.8,-152.7,-26.4,56.5 11 | 123.2,-152.6,-26.3,56.6 12 | 123.2,-152.4,-26.2,56.8 13 | 123.2,-152.1,-26.2,57.1 14 | 123.1,-151.8,-26.3,57.1 15 | 123.1,-152.2,-26.3,57.1 16 | 123,-152.5,-26.4,57 17 | 123.1,-152.4,-26.4,57 18 | 123.2,-152.6,-26.2,57.1 19 | 123.1,-152.9,-25.9,56.8 20 | 123.1,-152.9,-26,56.8 21 | 123,-152.7,-26,56.7 22 | 122.8,-152.4,-25.9,56.7 23 | 122.7,-152.1,-27.2,57 24 | 122.8,-151.4,-27.9,57.7 25 | 122.9,-151.1,-28.1,57.8 26 | 123,-150.6,-28.2,57.7 27 | 123,-150.1,-28.3,57.6 28 | 123,-150.5,-28.1,57.7 29 | 122.9,-151.3,-27.2,57.7 30 | 122.9,-152.6,-26.5,57.1 31 | 122.9,-153.4,-25.9,57 32 | 122.8,-154.3,-25.5,57.3 33 | 122.8,-154.9,-25.4,57.3 34 | 122.6,-154.7,-25.5,57 35 | 122.5,-154.1,-25.8,56.6 36 | 122.5,-153.7,-25.8,56.3 37 | 122.4,-153.4,-25.9,56.3 38 | 122.4,-153.1,-26.2,55.9 39 | 122.8,-153.4,-26.1,55.9 40 | 123,-153.1,-26.4,55.8 41 | 122.9,-152.9,-26.4,56 42 | 122.5,-152.7,-26.4,56.3 43 | 122.4,-152.8,-26.3,56.2 44 | 122.3,-152.8,-26.6,56.4 45 | 122.2,-151.8,-26.8,57.4 46 | 122.1,-151,-27.7,57 47 | 122.1,-150.9,-27.3,57.2 48 | 121.7,-150.1,-27.7,57.4 49 | 121.3,-150,-27.3,57.4 50 | 121.8,-150.5,-27.5,57.3 51 | 121.9,-151.7,-27.4,56.3 52 | 122,-152.8,-26.3,56.6 53 | 122.5,-152.9,-26.7,56.4 54 | 122.8,-153.4,-26.7,56.3 55 | 122.3,-153.1,-26.7,56.2 56 | 121.7,-152.6,-27.3,56.4 57 | 121.6,-152.6,-27,56.6 58 | 121.4,-152.6,-26.5,56.7 59 | 121.7,-153.1,-26.4,56.6 60 | 122.1,-153.1,-25.9,56.9 61 | 122.4,-153,-26.2,57.1 62 | 122.7,-152.9,-26,57.4 63 | 123,-153,-26,57.6 64 | 123,-152.4,-26.8,57.2 65 | 122.8,-152.4,-27.6,58 66 | 122.7,-151.5,-27.7,58.7 67 | 122.7,-151.8,-28,58.5 68 | 122.7,-151.9,-27.4,58.3 69 | 122.7,-152.2,-27.6,58 70 | 122.6,-152.9,-27,58.5 71 | 122.5,-153.1,-26.5,57.6 72 | 122.5,-154,-26.1,56.9 73 | 122.3,-153.4,-26.4,56.7 74 | 122.3,-153.4,-26.1,56.6 75 | 122.5,-153.2,-26.2,57.4 76 | 122.2,-152.3,-26.9,54.4 77 | 122.2,-151.4,-27.5,55.5 78 | 122.1,-150.8,-27.6,56.2 79 | 122.2,-150.2,-27.8,56 80 | 122.2,-149.8,-27.3,56 81 | 122.2,-149.2,-27.3,55.8 82 | 122.3,-150.2,-27.3,58.4 83 | 122.2,-151.2,-26.9,57.4 84 | 122.2,-152,-26.2,56.6 85 | 121.9,-152.8,-26.6,56.8 86 | 122.1,-153,-26.6,56.4 87 | 122.1,-152.7,-26.9,56.6 88 | 122,-152.7,-26.5,56.8 89 | 122,-152.5,-26.6,56.7 90 | 121.8,-152.7,-26.3,56.7 91 | 121.8,-152.8,-26.3,56.7 92 | 122.5,-153.2,-26.6,57.3 93 | 123,-153.1,-26.4,57.5 94 | 123.1,-152.5,-27.3,57.6 95 | 123.2,-152.3,-26.6,57.9 96 | 123,-152.6,-26,58 97 | 123,-152.9,-26.4,58.6 98 | 122.6,-152.6,-26.5,58.7 99 | 122.6,-152.6,-26.5,58.4 100 | 122.6,-153.3,-25.3,58.3 101 | 122.4,-153.3,-26,57.9 102 | 122.2,-152.8,-26.8,57.8 103 | 122,-152.7,-26.2,57 104 | 122,-152.8,-26.2,56.9 105 | 121.9,-152.7,-26.3,57 106 | 122.1,-153.6,-26,57.2 107 | 122.5,-153.6,-26.3,57.1 108 | 122.8,-153.7,-26.4,57 109 | 123.2,-153.5,-26.6,57 110 | 123.4,-153.5,-26,56.9 111 | 123.3,-153.8,-25.8,56.9 112 | 123.1,-153.8,-25.8,56.6 113 | 122.8,-153.8,-26.1,56.5 114 | 122.7,-153.6,-25.5,56.7 115 | 122.5,-153.6,-25.5,56.6 116 | 122.2,-152.3,-25.5,56.9 117 | 122.2,-151.3,-24.9,57.7 118 | 121.9,-150.4,-24.6,58.8 119 | 122.2,-150.4,-24.2,59.1 120 | 121.9,-150.9,-24.5,59.1 121 | 121.6,-150.9,-24.8,59.3 122 | 122,-152.1,-24.7,59 123 | 122.2,-153.1,-25.5,58.1 124 | 122.6,-154.1,-25.3,57.3 125 | 122.4,-154.3,-25.3,57.3 126 | 122.9,-154,-25,57.2 127 | 123.3,-153.8,-25.3,57.1 128 | 123.3,-153.8,-25.6,57.1 129 | 123.1,-153.3,-26.4,57.3 130 | 122.8,-153.1,-27.3,57.2 131 | 122.5,-153,-26.5,57.1 132 | 122.6,-153.3,-26.1,57.3 133 | 122.5,-152.4,-24.9,57.6 134 | 122.4,-152.1,-25.5,57.4 135 | 122.3,-152.1,-24.9,57.1 136 | 122.3,-152,-24.9,56.9 137 | 122.2,-152,-25.5,56.9 138 | 122.2,-151.7,-26.7,56.6 139 | 122.4,-152.9,-26.7,56.3 140 | 122.2,-153.3,-26,56.5 141 | 122.1,-153.3,-26.7,56.7 142 | 121.9,-152.9,-26.8,56.8 143 | 121.9,-152.7,-26.7,56.7 144 | 121.7,-152.6,-26.5,56.8 145 | 121.6,-152.6,-26.5,56.5 146 | 121.8,-152.7,-26.4,56.7 147 | 122.2,-153,-26,56.6 148 | 122.5,-153.3,-26.2,56.5 149 | 122.8,-153.4,-26.3,56.5 150 | 123.1,-153.2,-26.6,56.4 151 | 122.7,-152.8,-27.2,56.7 152 | 122.5,-151.9,-27.2,58.1 153 | 122.3,-150.9,-26.8,59.3 154 | 122.1,-150.7,-26.7,60.2 155 | 122.1,-150.9,-26.9,60.2 156 | 121.9,-150.6,-27,60.2 157 | 122.4,-151.1,-26.8,59.9 158 | 122.6,-152.1,-26.7,58.4 159 | 122.7,-153.1,-27.3,57.1 160 | 122.9,-153.5,-27,56.3 161 | 122.9,-153.7,-26.8,56.1 162 | 123,-153.8,-26.8,56.3 163 | 123.1,-154.1,-26.5,56.5 164 | 123.2,-153.7,-26.5,56.6 165 | 123.5,-153.5,-26.9,56.8 166 | 123.3,-153.5,-26.5,56.9 167 | 122.9,-153.4,-26.6,57.3 168 | 122.6,-153.4,-26.8,57.5 169 | 121.7,-153.1,-26.9,57.5 170 | 121.7,-152.5,-27.3,57.4 171 | 121.7,-152.3,-27,57.4 172 | 121.9,-152.4,-26.4,57 173 | 122,-151.5,-26.4,56.5 174 | 122.1,-151.1,-25.2,56.3 175 | 122,-150.7,-25.5,56.1 176 | 122.1,-150.4,-25.5,55.9 177 | 122.1,-150.7,-25.6,55.9 178 | 122.1,-150.7,-26.1,56.3 179 | 122.1,-151.6,-26.2,56.7 180 | 122.3,-152.2,-27.5,56.9 181 | 122.3,-152.6,-27.4,57.3 182 | 122.3,-153.1,-27.2,57.5 183 | 122.3,-152.6,-27.1,57.3 184 | 122.2,-152.5,-26.9,57.2 185 | 122.3,-152.6,-26.5,57 186 | 122.4,-152.5,-26.4,56.9 187 | 122.4,-152.6,-26,56.8 188 | 122.7,-152.5,-26,56.9 189 | 122.8,-153,-26.1,57.4 190 | 123,-153.2,-26.1,57.4 191 | 122.9,-153.8,-26,57.2 192 | 123.1,-153.9,-26.1,57.1 193 | 123.4,-153.7,-26.7,57.1 194 | 123.6,-152.3,-27.1,56.8 195 | 123.3,-151.7,-27.6,56.5 196 | 123,-151,-27.2,56.4 197 | 122.9,-150.4,-27.2,56.1 198 | 122.8,-149.9,-27.2,55.9 199 | 122.7,-149.9,-27.5,55.7 200 | 122.8,-150.9,-27.3,55.6 201 | -------------------------------------------------------------------------------- /dataset_roh/(X4.5,Y4.5).csv: -------------------------------------------------------------------------------- 1 | 134.1,-162.8,-43.6,69 2 | 134.4,-162.7,-44.1,69.4 3 | 134.3,-162.3,-44.3,69.9 4 | 134,-162,-44.8,70 5 | 133.8,-161.7,-45.1,70.3 6 | 133.8,-161.5,-45.8,70.5 7 | 133.4,-161.2,-46.6,71.1 8 | 133.1,-160.9,-46.3,71.5 9 | 132.8,-161,-45.9,71.9 10 | 133.1,-161,-45.8,72.2 11 | 133.2,-161.1,-45.5,72.3 12 | 133.1,-161.1,-45.3,72.4 13 | 133.4,-161.2,-44.7,72.2 14 | 133.7,-161.2,-44.6,72 15 | 133.9,-161.7,-45.3,72 16 | 133.7,-161.5,-45.1,72.4 17 | 133.7,-161.3,-44.7,73 18 | 133.9,-161.6,-43.9,72.9 19 | 133.9,-161.4,-43.3,72.7 20 | 134,-161.3,-43.3,72.1 21 | 134.3,-162,-43.9,72.5 22 | 136.2,-162.3,-46.5,73.8 23 | 133.9,-163.2,-43.2,71.1 24 | 134.2,-162.4,-42.9,70.7 25 | 134.4,-162.6,-43.4,70.1 26 | 135,-160.3,-44.6,74.3 27 | 135.8,-158.5,-44.4,76.3 28 | 133.9,-161.4,-42.9,71.2 29 | 134,-161.8,-42.7,70.6 30 | 134.5,-162.4,-42.5,70 31 | 134.5,-162.5,-43.3,69.8 32 | 134.3,-162.2,-43.9,69.8 33 | 134.2,-161.9,-44.3,69.7 34 | 134.1,-161.8,-45.4,69.9 35 | 134,-161.3,-44.6,69.4 36 | 134,-161.5,-44.5,70.1 37 | 134.1,-162.2,-44.8,70.5 38 | 134.2,-162.5,-44.7,70.6 39 | 134.2,-162.2,-45.9,71.5 40 | 134.2,-161.6,-45.5,72.1 41 | 134.1,-161.9,-45.7,72.5 42 | 133.8,-161.9,-45.6,72.6 43 | 134,-161.8,-45.7,72.3 44 | 134,-161.8,-46.1,72.2 45 | 134.8,-158.8,-46.1,74.3 46 | 134.5,-155.9,-46.3,73.7 47 | 133.9,-161.3,-45.5,71.9 48 | 136,-156.9,-44.6,73.7 49 | 136.3,-159.3,-40.1,72.7 50 | 133.9,-155.8,-40.1,71.7 51 | 134.1,-157.1,-47.1,70.3 52 | 134.1,-158,-47.1,69.2 53 | 136.7,-158.9,-46.1,74.6 54 | 133.5,-163,-45.1,71.8 55 | 134.4,-163.7,-44.3,70.6 56 | 134.4,-161.7,-45.2,70.7 57 | 134.3,-161.8,-45.2,70.9 58 | 134.6,-162,-44.7,71.2 59 | 134.4,-161.8,-45.1,71.3 60 | 134.2,-161.7,-44.7,71.7 61 | 134,-160.2,-43.6,76.6 62 | 134.3,-163.5,-44.1,72.3 63 | 134.4,-163.1,-43.9,70.2 64 | 134.3,-162.8,-44,69.7 65 | 134.3,-162.4,-44.4,69.8 66 | 133.9,-162.3,-44.4,70.3 67 | 133.9,-162.1,-44.3,70.8 68 | 133.9,-162,-45.1,70.9 69 | 133.9,-162,-45.3,70.7 70 | 133.8,-162,-45.2,70.8 71 | 133.9,-162,-45,71 72 | 134.1,-162,-45.1,71.1 73 | 134.3,-162.1,-44.8,70.9 74 | 134.5,-162.1,-44.8,71.3 75 | 134.3,-162.1,-45,71.3 76 | 134.6,-162.1,-45,71.3 77 | 134.6,-161.9,-44.9,71.3 78 | 134.6,-162.6,-45,71.5 79 | 134.3,-162.7,-44.9,71.4 80 | 134.2,-163,-45,72.1 81 | 134.1,-163.1,-44.9,72.2 82 | 134,-162.9,-44.5,72.4 83 | 133.7,-163.2,-44.2,72.5 84 | 133.7,-162.7,-44.6,71.4 85 | 133.9,-162.7,-44.8,71.2 86 | 133.9,-162.2,-44.8,72 87 | 134,-162.2,-44.8,72 88 | 134.2,-162.3,-45,72.1 89 | 134.6,-159.6,-45.3,74.4 90 | 133.8,-163.1,-43.2,76.6 91 | 134,-163.5,-43.6,75.5 92 | 137.1,-159.9,-40.4,74.2 93 | 134.1,-163.6,-46.9,71.2 94 | 134.1,-163.2,-46.3,70.5 95 | 134.2,-161.8,-44.5,69.4 96 | 133.9,-161.7,-44.4,69.6 97 | 133.9,-161.7,-44.4,69.7 98 | 134,-162,-44.6,69.8 99 | 134.3,-162,-44.5,69.9 100 | 134.4,-162,-44.7,70 101 | 134.3,-162.2,-44.9,70.2 102 | 134.3,-162.2,-44.9,70.1 103 | 134,-162.3,-45.4,70.4 104 | 133.9,-162.3,-45.4,70.7 105 | 133.9,-162.3,-45.3,71.1 106 | 133.5,-161.9,-45.7,72 107 | 133.8,-162.3,-46.1,72.7 108 | 134.1,-162.6,-46.4,72.9 109 | 134.5,-162.7,-46.2,72.9 110 | 134.7,-162.7,-46.3,73.1 111 | 134.6,-162.8,-46.7,73.5 112 | 134.9,-163.1,-46.5,73 113 | 134.6,-162.7,-45.9,72.5 114 | 134.2,-162.4,-45.6,72.4 115 | 134.1,-162.5,-45.4,72.2 116 | 133.8,-162.2,-45.2,72 117 | 133.8,-162.2,-44.9,71.7 118 | 133.6,-162.4,-44.9,71.6 119 | 133.8,-162.5,-44.9,71.6 120 | 134,-162.2,-44.8,71.2 121 | 134.2,-162.2,-45,70.9 122 | 134.3,-161.9,-44.9,70.8 123 | 134.2,-161.7,-44.9,70.6 124 | 134.2,-161.7,-44.9,70.6 125 | 134.2,-161.7,-44.7,70.4 126 | 134.2,-161.8,-44.7,70.7 127 | 134.1,-161.8,-44.6,71 128 | 134.1,-162.2,-44.5,70.9 129 | 134.1,-162.2,-44.3,71 130 | 134,-162.2,-44.2,70.8 131 | 134,-162.2,-44.3,70.8 132 | 134.2,-162.2,-44.4,70.8 133 | 133.8,-161.5,-43.7,71.2 134 | 134,-161,-44.1,72.6 135 | 134.3,-161.3,-44.2,72.7 136 | 134.6,-161.4,-44.9,72.7 137 | 134.5,-161.6,-45.5,72.2 138 | 134.5,-161.3,-45.6,72 139 | 134.7,-161.8,-46.5,71.4 140 | 134.4,-162.1,-46.3,69.8 141 | 134.1,-161.9,-46.5,69.5 142 | 134,-161.9,-45.7,69.5 143 | 134.1,-161.4,-45.2,69.9 144 | 134.1,-161.8,-45.2,70.2 145 | 134.1,-162.1,-44.9,69.9 146 | 134.1,-162.1,-44.9,69.7 147 | 134.1,-162.1,-44.7,69.7 148 | 134.3,-161.9,-45.3,69.8 149 | 134.2,-161.9,-45.3,69.6 150 | 134.2,-161.9,-45.2,69.3 151 | 134.2,-162,-45.5,69.7 152 | 134.1,-162.1,-45.5,69.7 153 | 134.4,-162.1,-45.4,69.8 154 | 134.1,-162.1,-45,69.8 155 | 134.1,-162.2,-45,70.1 156 | 134.1,-162.2,-44.9,70.2 157 | 134.1,-162.2,-44.6,70.2 158 | 134.1,-162.1,-44.5,70.5 159 | 133.7,-162.1,-44.5,70.4 160 | 133.7,-162.2,-44,71 161 | 133.7,-161.7,-43.5,71.4 162 | 133.6,-161.7,-43.5,71.9 163 | 133.5,-161.8,-43.3,72.1 164 | 133.5,-161.9,-44,71.9 165 | 133.7,-161.9,-44.5,71.9 166 | 134,-162.3,-44.5,71.2 167 | 134.2,-162.6,-44.7,71.1 168 | 134.2,-162.6,-44.8,70.6 169 | 134.4,-162.3,-45.1,70.4 170 | 134.7,-162.3,-44.3,70.4 171 | 134.8,-158,-44.5,73.3 172 | 134.8,-157.9,-47.1,72.4 173 | 134.4,-162.9,-45.6,70.5 174 | 134.5,-163.5,-44.9,69.9 175 | 134.5,-163.1,-44.9,69.6 176 | 134.5,-162,-45.1,69.2 177 | 134.6,-161.9,-44.9,69.6 178 | 134.1,-162,-45.4,73 179 | 134.6,-163.1,-43.8,70.5 180 | 134.6,-163.5,-43,69.7 181 | 134.3,-163.1,-43.3,69.8 182 | 134.1,-162.4,-44.3,69.9 183 | 134.1,-161.9,-45.2,70.1 184 | 134.4,-158.4,-44.2,73.2 185 | 134.2,-163.3,-44,69.6 186 | 134.2,-163.7,-43.4,69.3 187 | 134.4,-162.8,-43.9,69.1 188 | 134.4,-162.6,-43.8,68.9 189 | 134.1,-162.5,-44.5,69.3 190 | 134.1,-162.3,-44.7,69.6 191 | 134.3,-161.9,-45.4,69.7 192 | 134.3,-162,-46,69.8 193 | 134.3,-162,-45.9,70 194 | 134.3,-161.9,-46,70.4 195 | 134.5,-162,-45.5,70.2 196 | 134.2,-162,-45.6,70.4 197 | 133.9,-162,-45.5,70.4 198 | 133.9,-162.1,-45.8,70.5 199 | 133.9,-162.2,-45.6,70.5 200 | 133.8,-162.3,-45.4,70.6 201 | -------------------------------------------------------------------------------- /dataset_roh/(X3.0,Y4.5).csv: -------------------------------------------------------------------------------- 1 | 142.4,-149.1,-66.6,63.2 2 | 142.4,-149.2,-66.6,62.1 3 | 142.4,-148.9,-66.6,61.1 4 | 142.4,-149.4,-66.7,61.5 5 | 142.4,-149.2,-66.5,61.8 6 | 142.6,-149.4,-66.3,62.9 7 | 142.6,-149.4,-66.1,63.8 8 | 142.8,-149.3,-65.8,64.5 9 | 143.1,-149,-65.6,64.6 10 | 143.3,-149,-65.6,63.8 11 | 143.5,-148.7,-66,63.1 12 | 143.5,-148.5,-66,62.3 13 | 143.8,-148.2,-66.4,62.1 14 | 143.7,-148.1,-66.5,62.2 15 | 143.7,-148.1,-66.6,62.4 16 | 143.6,-148,-66.5,62.9 17 | 143.6,-148.1,-66.2,62.9 18 | 143.6,-148.1,-66.1,62.7 19 | 143.5,-148.2,-65.8,62.6 20 | 143.5,-148.2,-65.5,62.6 21 | 143.4,-148.2,-65.5,63.2 22 | 143.4,-148.2,-65.5,62.9 23 | 143.3,-148.7,-65.6,63.4 24 | 143.2,-148.7,-66.6,64.9 25 | 143.1,-149,-67.3,65 26 | 143.4,-149.1,-68,64.9 27 | 143.6,-149.2,-68.4,63.9 28 | 143.8,-149,-68.5,63.9 29 | 144.1,-148.4,-68.8,63.9 30 | 144.3,-148.1,-67.9,62.8 31 | 144.6,-147.7,-67.3,63 32 | 144.5,-147.4,-66.8,63.4 33 | 144.4,-147.1,-66.5,64.4 34 | 144.3,-147.2,-66.4,64.2 35 | 144.2,-147.2,-66.1,64.3 36 | 144.2,-147.3,-65.9,64.3 37 | 144.1,-147.3,-65.7,64 38 | 144.1,-147.3,-65.6,64.5 39 | 143.9,-147.4,-65.5,63.7 40 | 143.9,-147.5,-65.4,64.2 41 | 143.9,-147.5,-65.1,64 42 | 143.7,-147.5,-65.4,63.8 43 | 143.7,-147.6,-65.1,63.6 44 | 143.7,-147.6,-64.9,62.5 45 | 143.8,-147.7,-64.8,62.5 46 | 143.9,-147.7,-64.8,61.8 47 | 143.7,-147.7,-64.8,61.8 48 | 143.8,-147.8,-64.5,61.9 49 | 143.8,-147.8,-64.6,61.8 50 | 143.7,-148.2,-65.4,62.4 51 | 143.9,-148.4,-65.8,62.9 52 | 143.8,-148.2,-66.9,63.8 53 | 143.9,-148,-66.8,64.4 54 | 143.8,-148.1,-67,64.7 55 | 143.7,-148.1,-67.2,64.1 56 | 143.7,-147.8,-67,63.2 57 | 143.5,-147.6,-67,63.4 58 | 143.6,-147.8,-66.1,62.5 59 | 143.8,-147.9,-66.6,62.2 60 | 143.8,-147.8,-66.4,61.6 61 | 143.8,-147.6,-66.4,62.6 62 | 143.6,-147.4,-66.1,63.7 63 | 143.5,-147.3,-65.9,63.7 64 | 143.3,-147.1,-65.8,63.9 65 | 143.2,-146.9,-65.6,64.1 66 | 143.2,-146.8,-65.6,64.5 67 | 143.2,-146.6,-65.6,64.4 68 | 143.4,-146.8,-65.4,64.2 69 | 143.5,-146.9,-65.3,64.1 70 | 143.7,-147.1,-65.1,64.5 71 | 143.8,-147.2,-65.1,64.4 72 | 144,-147.7,-65.1,64.4 73 | 144.2,-147.8,-65.3,64.5 74 | 144.2,-147.8,-65.3,64.1 75 | 144.2,-148,-65.2,63.6 76 | 144,-148.1,-65.3,63.2 77 | 143.9,-148.2,-65.3,62.9 78 | 143.6,-148.1,-66,63 79 | 143.5,-148.4,-66,63.8 80 | 143.5,-148.2,-67.7,65.3 81 | 143.5,-148.2,-68.1,66.5 82 | 143.6,-148.2,-68.4,67.7 83 | 143.6,-147.8,-68.7,69.6 84 | 143.8,-147.8,-68.2,70.8 85 | 143.8,-147.7,-68.2,71.1 86 | 143.8,-147.8,-66.9,70.9 87 | 143.8,-147.7,-66.4,71.2 88 | 143.8,-147.5,-66.1,71.4 89 | 144,-147.7,-65.9,71.4 90 | 144.1,-147.5,-65.6,70.9 91 | 144.3,-147.4,-65.6,70.7 92 | 144.5,-147.2,-65.4,70.2 93 | 144.6,-147.2,-65.4,68.7 94 | 144.5,-147.4,-65.4,67.8 95 | 144.3,-147.5,-65.3,65.8 96 | 144.2,-147.7,-65.6,64.3 97 | 144.1,-147.8,-65.4,62.9 98 | 143.9,-147.9,-65.3,61.9 99 | 143.5,-148.2,-65.4,61.8 100 | 143.3,-148.4,-65.7,61.2 101 | 143.2,-148.2,-65.9,61.3 102 | 143,-148.4,-65.7,61.5 103 | 142.9,-148.5,-65.9,61.5 104 | 142.7,-148.6,-66,62.1 105 | 142.7,-148.2,-66.2,63.6 106 | 142.9,-147.3,-66.2,65 107 | 142.9,-147,-65.9,66 108 | 142.9,-146.8,-65.4,66.4 109 | 142.9,-146.5,-65.1,66.4 110 | 142.9,-146.5,-65.2,65.8 111 | 143,-146.8,-65.4,66.3 112 | 143.1,-147.5,-65.8,66.7 113 | 143.2,-148,-66.2,66.7 114 | 143.4,-148,-66.7,68.2 115 | 143.4,-148.1,-67,69.8 116 | 143.9,-148,-67,71.1 117 | 143.7,-148,-66.9,71.4 118 | 143.6,-147.8,-66.2,71 119 | 143.4,-147.6,-66.1,70.3 120 | 143.2,-147.8,-66.1,68.6 121 | 143.3,-147.9,-66.1,67 122 | 142.8,-148,-65.9,64.8 123 | 143,-148,-65.7,63.8 124 | 143.1,-148.2,-65.7,63.1 125 | 143.3,-148.3,-65.6,62.6 126 | 143.4,-148.2,-65.4,62.2 127 | 143.6,-148.1,-65.4,61.9 128 | 143.7,-147.9,-65.6,62 129 | 143.9,-147.7,-65.6,62.2 130 | 144.1,-147.6,-65.6,61.6 131 | 144.2,-147.4,-65.7,61.7 132 | 144.4,-147.2,-65.7,61.8 133 | 144,-147.5,-65.5,62 134 | 143.9,-147.8,-64.9,63.6 135 | 143.9,-148.1,-64.8,64.8 136 | 143.9,-148.4,-64.4,66.1 137 | 143.8,-148.4,-64.4,67.3 138 | 143.8,-148.4,-65.2,68.2 139 | 144,-147.9,-65.5,68.4 140 | 144,-147.5,-66.1,68.6 141 | 143.8,-147.3,-66.5,68.7 142 | 143.7,-147.2,-67.1,68.9 143 | 143.7,-147.3,-67,69 144 | 143.5,-147.5,-66.2,69.5 145 | 143.7,-147.6,-65.9,70.6 146 | 143.9,-147.7,-65.6,70.3 147 | 144,-147.6,-65.3,70.3 148 | 144.2,-147.4,-64.9,70.3 149 | 144.4,-147.6,-64.9,70.4 150 | 144.5,-147.4,-64.9,70.9 151 | 144.7,-147.6,-64.9,71.4 152 | 144.6,-147.4,-64.9,70 153 | 144.4,-147.6,-64.9,68.7 154 | 144.3,-147.7,-65.2,67.1 155 | 144.1,-147.6,-65.5,65.6 156 | 144,-147.9,-65.5,63.8 157 | 143.6,-148,-65.8,61.8 158 | 143.4,-148.5,-66.1,61.8 159 | 143.3,-148.6,-66.2,62.3 160 | 143.1,-148.8,-65.8,62.6 161 | 143.1,-148.5,-65.7,62.7 162 | 143.3,-148.3,-65.4,63.9 163 | 143.3,-147.7,-65.3,65 164 | 143.4,-147.3,-65.3,65.9 165 | 143.4,-147,-65,66.2 166 | 143.5,-147,-65.3,66.6 167 | 143.4,-147.4,-65.9,68.1 168 | 143.3,-147.6,-66.6,68.4 169 | 143.4,-148,-66.7,69 170 | 143.5,-148.2,-66.9,69.4 171 | 143.6,-148.3,-67.3,70 172 | 143.5,-148.1,-67.2,69 173 | 143.6,-148,-66.9,66.9 174 | 143.7,-147.8,-66.4,65 175 | 143.6,-147.6,-66.3,63.7 176 | 143.6,-147.5,-66.1,62.8 177 | 143.6,-147.3,-66,61.7 178 | 143.6,-147.2,-66,62.6 179 | 143.6,-147.1,-66,63.4 180 | 143.7,-147,-65.8,64.3 181 | 143.7,-146.9,-65.8,64.4 182 | 143.7,-146.7,-65.9,64.4 183 | 143.8,-146.9,-65.9,64 184 | 143.8,-147,-66.1,64 185 | 143.8,-147.2,-66.3,63.6 186 | 143.8,-147.5,-66.3,63.1 187 | 144,-147.9,-66.3,62.7 188 | 143.7,-148.5,-65.9,62.7 189 | 143.7,-148.5,-65.8,63.9 190 | 143.6,-148.4,-65.2,64.7 191 | 143.6,-148.7,-64.8,65.6 192 | 143.5,-148.5,-65,66.5 193 | 143.5,-148.2,-65,67.4 194 | 143.9,-147.7,-65.7,67.4 195 | 144.1,-147.6,-65.8,67.9 196 | 144.4,-147.5,-66.6,68 197 | 144.2,-147,-67,68.5 198 | 144.2,-146.9,-67,69.1 199 | 144,-147,-66.3,70.2 200 | 143.8,-147.1,-66.1,68.4 201 | -------------------------------------------------------------------------------- /dataset_roh/(X4.5,Y1.5).csv: -------------------------------------------------------------------------------- 1 | 106,-126.8,-16.8,46 2 | 106.4,-127,-16.5,45.7 3 | 106.9,-126.7,-16.4,45.6 4 | 107.1,-126.3,-16.5,45.8 5 | 107.2,-126,-16.2,45.9 6 | 107.3,-125.4,-16.2,46.2 7 | 107.2,-125.8,-16.3,46.4 8 | 107.3,-126.1,-16.3,46.7 9 | 107.1,-126.8,-16.6,46.6 10 | 107.4,-128.7,-16.8,46.5 11 | 107.3,-129.1,-16.8,46.5 12 | 107.5,-130.7,-16.8,46.4 13 | 107.5,-130.8,-16.6,46.3 14 | 107.4,-131.1,-16.7,46.2 15 | 107.4,-131.2,-16.8,46.1 16 | 107.3,-131.3,-16.8,46 17 | 107.2,-131.2,-16.8,45.9 18 | 106.8,-131.3,-16.9,46.1 19 | 106.4,-131.1,-17,46.3 20 | 106.1,-131.2,-17.1,46.6 21 | 105.8,-130.9,-17.1,46.8 22 | 105.7,-131,-17.2,46.7 23 | 105.7,-130.9,-17.3,47 24 | 106,-130.9,-17.2,46.7 25 | 106.2,-130.9,-17.3,46.4 26 | 106.5,-131.2,-17.3,46.2 27 | 106.7,-131.3,-17.4,45.9 28 | 106.9,-131.5,-17.1,46.1 29 | 106.9,-131.6,-16.7,45.8 30 | 107.1,-131.5,-16.5,45.7 31 | 107.5,-131.7,-16.5,45.6 32 | 107.6,-131.8,-16.2,45.4 33 | 108,-131.6,-16.4,45.3 34 | 108.2,-131.5,-16.6,44.9 35 | 107.8,-131.1,-16.9,45.2 36 | 107.7,-131.1,-16.8,45.1 37 | 107.7,-131.6,-16.9,45 38 | 107.6,-131.7,-17,45.7 39 | 107.3,-130.3,-17.4,45.9 40 | 107.1,-130.4,-17.7,45.9 41 | 107.1,-130.1,-17.8,46.1 42 | 107,-129.7,-18,46.7 43 | 107,-129.1,-18,47 44 | 107.2,-129.4,-17.8,46.9 45 | 107.4,-130.5,-17.4,47 46 | 107.7,-130.4,-17.2,46.8 47 | 108,-130.2,-17.2,46.9 48 | 108,-130.4,-17.1,46.7 49 | 107.9,-130.7,-17.1,46.6 50 | 107.8,-131.8,-17.2,46.3 51 | 107.5,-132.4,-17.4,46.2 52 | 107.7,-132.7,-17.5,46.4 53 | 107.7,-132.8,-17.5,46.3 54 | 107.5,-132.9,-17.4,46.2 55 | 107.7,-132.4,-17.1,45.8 56 | 107.7,-132.3,-16.9,45.8 57 | 107.7,-132.2,-16.8,45.7 58 | 107.6,-131.7,-16.6,45.6 59 | 107.4,-131.4,-16.7,45.5 60 | 107.4,-131.4,-15.6,45.6 61 | 107.3,-131.2,-15.8,46 62 | 107.2,-131.2,-16.1,46.4 63 | 107.2,-131,-16.4,46.8 64 | 107.3,-131,-16.2,46.8 65 | 107.5,-130.8,-16.3,46.9 66 | 107.5,-130.8,-17.5,46.8 67 | 107.7,-130.9,-17.5,46.6 68 | 107.7,-131.1,-17.3,46.5 69 | 107.7,-131.2,-17.2,46.4 70 | 107.7,-131.4,-17.4,46.5 71 | 107.4,-131.5,-17.2,46.7 72 | 107.1,-131.4,-17,46.8 73 | 107.3,-131.3,-16.7,47 74 | 107.5,-131.2,-16.6,47 75 | 107.6,-131.3,-16.7,46.8 76 | 107.7,-131.1,-16.9,46.7 77 | 107.9,-131.1,-17.2,46.6 78 | 107.9,-130.8,-16.9,46.5 79 | 107.9,-131,-16.7,46.4 80 | 108.1,-131,-16.5,46.6 81 | 108.1,-131.3,-16.8,46.9 82 | 107.8,-131.4,-17.3,47 83 | 107.6,-130.9,-17.8,47.1 84 | 107.5,-131.6,-18.3,47 85 | 107.3,-131.3,-18.4,46.7 86 | 107.3,-131.3,-18.4,46.4 87 | 107.5,-131.6,-18.6,46.1 88 | 107.8,-131.4,-18.3,46 89 | 108.2,-131.6,-18,45.9 90 | 108.2,-130.8,-17.8,46.2 91 | 108.2,-130.9,-17.7,46.4 92 | 108.1,-130.8,-17.7,46.2 93 | 108.1,-130.3,-17.5,46.4 94 | 108.1,-130.4,-17.3,46.5 95 | 108.1,-130.6,-17.2,46.7 96 | 108,-130.7,-17.2,46.6 97 | 107.7,-131.3,-17.2,46.7 98 | 107.4,-131.8,-17.2,46.9 99 | 107.2,-132.1,-17.3,46.8 100 | 107.6,-132.3,-17.4,46.8 101 | 107.9,-132.1,-17.7,46.5 102 | 107.9,-132.1,-17.7,46.1 103 | 108,-131.6,-17.5,46.1 104 | 107.9,-130.4,-17.2,46.3 105 | 107.8,-128.9,-16.9,46.4 106 | 107.8,-127.6,-17,46.2 107 | 107.8,-126.4,-17,46.3 108 | 107.8,-125.9,-17.1,46.4 109 | 107.8,-125,-17.2,46.1 110 | 108.3,-127.4,-17.6,46 111 | 108.6,-128.2,-17.4,45.9 112 | 108.4,-129.9,-17.3,46.1 113 | 108.3,-129.7,-17.4,46.3 114 | 107.8,-131.3,-17.4,46.6 115 | 107.7,-131.8,-17.6,46.9 116 | 107.4,-131.8,-17.2,47.1 117 | 107.2,-131.7,-17.1,47.1 118 | 107.5,-131.2,-16.9,46.8 119 | 107.8,-131.2,-16.7,46.8 120 | 108.1,-131.7,-16.6,46.5 121 | 107.8,-131.2,-17.2,46.3 122 | 107.5,-129.5,-17.7,46.1 123 | 107.5,-127.6,-18.1,46.1 124 | 107.3,-125.2,-18.5,46.2 125 | 107.2,-124,-18.8,45.9 126 | 107.1,-122.5,-18.8,46 127 | 107.6,-121.7,-18.3,46.1 128 | 108,-122.2,-18,46.3 129 | 108,-123.1,-17.5,46.2 130 | 108.3,-122.6,-17.4,46.6 131 | 108.2,-122.9,-17.4,46.9 132 | 108.1,-123.6,-17.4,47.3 133 | 107.9,-125.3,-17.5,47.2 134 | 108,-126.1,-17.5,47 135 | 107.8,-130.1,-17.7,46.8 136 | 107.8,-131.5,-17.7,46.4 137 | 107.9,-131.8,-17.4,46.2 138 | 108,-131.6,-17.2,45.8 139 | 108.2,-131.8,-17,45.8 140 | 108.3,-131.5,-16.7,45.8 141 | 108.4,-131.5,-16.6,46.1 142 | 108.1,-130.3,-16.9,46.1 143 | 107.9,-129.7,-18.1,46.2 144 | 107.8,-128.8,-18.5,46.3 145 | 107.7,-127.8,-18.7,46.4 146 | 107.5,-126.8,-18.8,46.4 147 | 107.5,-126.5,-18.6,46.5 148 | 107.7,-125.7,-18.6,46.8 149 | 107.9,-125.3,-17.6,46.6 150 | 108,-126.3,-17.3,46.5 151 | 108.1,-126.2,-17.3,46.3 152 | 108.3,-125.7,-17.2,46.3 153 | 108.3,-125.9,-17.2,46.2 154 | 108.5,-126,-17,46 155 | 108.5,-127.2,-17,46.1 156 | 108.3,-127.1,-16.9,46.3 157 | 108.5,-129.5,-16.7,46.5 158 | 108.2,-131.5,-16.8,46.6 159 | 108.1,-131.6,-17,46.8 160 | 108.1,-131.6,-17.2,46.7 161 | 108.3,-131.4,-17,46.7 162 | 108.4,-131.1,-16.8,46.7 163 | 108.3,-131.3,-16.8,46.4 164 | 107.8,-131,-17,46.1 165 | 107.5,-128.9,-17.3,45.8 166 | 107.2,-126.7,-17.5,45.9 167 | 107,-126.1,-17.8,45.7 168 | 106.8,-124.8,-17.9,45.7 169 | 106.7,-123.5,-18.2,46 170 | 107,-122.4,-17.9,46.3 171 | 107.3,-123.4,-17.8,46.5 172 | 107.4,-124.6,-17.4,46.7 173 | 107.4,-124.5,-17.3,47.1 174 | 107.4,-124,-17,47.1 175 | 107.4,-125.1,-16.9,47 176 | 107.4,-125.2,-17,46.8 177 | 107.2,-126.3,-17.2,46.6 178 | 107.2,-127.4,-17.5,46.5 179 | 107.4,-128.4,-17.5,46.3 180 | 107.7,-130.2,-17.8,46 181 | 108.1,-131.5,-17.6,45.9 182 | 108.3,-131.1,-17.4,45.9 183 | 108.5,-131.1,-17.2,45.8 184 | 108.5,-130.9,-16.9,45.8 185 | 108.5,-131.1,-17,45.9 186 | 108.2,-130.5,-15.6,46 187 | 108,-129.2,-16.2,46.5 188 | 107.7,-129.2,-16.8,46.6 189 | 107.6,-128.9,-17.1,46.7 190 | 107.5,-128.9,-17.5,46.7 191 | 107.5,-129.4,-17.6,46.7 192 | 107.6,-129.8,-18.9,46.9 193 | 107.8,-130.9,-18.5,46.8 194 | 108,-130.7,-18.1,46.9 195 | 108.1,-131,-17.8,47.1 196 | 108,-131.1,-17.3,46.9 197 | 108,-130.7,-17.3,47 198 | 107.7,-130.8,-17.4,46.8 199 | 107.6,-131,-17.2,46.7 200 | 107.4,-131.6,-17.2,46.3 201 | -------------------------------------------------------------------------------- /dataset_roh/(X3.0,Y3.0).csv: -------------------------------------------------------------------------------- 1 | 133.6,-134.9,-45.1,46.8 2 | 133.3,-134.5,-44.5,46.5 3 | 133.3,-133.9,-44.2,46.2 4 | 133.3,-133.6,-43.6,46.1 5 | 133.2,-133.4,-43.5,45.7 6 | 133.2,-133.3,-43.5,45.4 7 | 133.3,-133.3,-43.4,45.1 8 | 133.4,-133,-43.3,45.4 9 | 133.9,-132.5,-43.1,45.8 10 | 134.1,-132.2,-43.2,46 11 | 134.1,-132.2,-43.2,46 12 | 134.1,-132.2,-43.2,46 13 | 134.6,-131.7,-42.8,46.5 14 | 134.6,-131.7,-43,46.5 15 | 134.5,-131.9,-43,46.4 16 | 134.5,-132,-42.9,46.4 17 | 134.7,-132.1,-43.1,46.3 18 | 134.7,-132.1,-43.1,46.3 19 | 134.6,-132.3,-43.3,46.2 20 | 134.6,-132.4,-43.4,46.1 21 | 134.5,-132.6,-44,45.7 22 | 134.7,-132.9,-44.2,45.3 23 | 134.3,-133.1,-44.3,45.1 24 | 134.3,-133.3,-44.3,45 25 | 134.2,-133.7,-44.6,44.9 26 | 134.5,-134.1,-44.5,45.1 27 | 134.6,-134.3,-43.9,45.7 28 | 134.4,-134.2,-43.7,46.3 29 | 134.5,-134,-43.4,46.7 30 | 134.8,-133.8,-43.1,47 31 | 134.7,-133.4,-42.9,47.3 32 | 134.7,-133.1,-42.7,46.9 33 | 134.6,-132.8,-42.8,46.9 34 | 134.4,-132.9,-42.8,46.5 35 | 134.4,-132.9,-42.7,46.2 36 | 134.2,-132.9,-42.9,46.1 37 | 134.3,-133,-42.8,46.1 38 | 134.1,-133,-42.9,46.3 39 | 134.1,-133,-42.9,46.3 40 | 134.4,-133.1,-43,46.5 41 | 134.6,-133,-43.2,46.8 42 | 134.9,-133.2,-42.8,47 43 | 135.2,-133.2,-42.5,47.3 44 | 134.7,-133.9,-43,47.1 45 | 134.3,-133.3,-43.2,47 46 | 133.9,-132.8,-43.4,46.9 47 | 133.7,-132.9,-43.8,46.5 48 | 133.3,-132.6,-44.1,46.1 49 | 133,-132.4,-44.3,45.8 50 | 133.5,-132,-43.5,46.1 51 | 133.7,-132.6,-43.3,46.2 52 | 133.8,-133.1,-43.3,46.3 53 | 133.8,-132.9,-43.2,46.2 54 | 133.8,-133.1,-43,46 55 | 133.8,-133.2,-43,45.9 56 | 133.9,-133,-43,45.7 57 | 134.3,-132.9,-42.9,45.6 58 | 134.2,-132.7,-42.7,45.6 59 | 134.3,-132.6,-42.6,45.8 60 | 134.4,-132.4,-42.6,45.9 61 | 134.6,-132.2,-42.7,46.1 62 | 134.5,-132.2,-42.9,46.3 63 | 134.4,-132.6,-43.2,46.4 64 | 134.6,-132.9,-43.5,46.4 65 | 134.6,-133,-43.5,46.7 66 | 134.8,-132.7,-43.4,46.9 67 | 134.8,-132.8,-43.4,47 68 | 134.7,-132.2,-43.5,46.6 69 | 134.7,-131.9,-43.8,46.4 70 | 134.7,-132.2,-44,46.2 71 | 134.9,-132.4,-44.1,46 72 | 134.6,-132.5,-44.4,45.7 73 | 134.8,-132.6,-44.2,45.6 74 | 134.9,-133.3,-44.1,45.8 75 | 134.9,-133.8,-43.9,45.6 76 | 134.9,-133.7,-43.9,45.4 77 | 134.6,-133.5,-43.9,45.3 78 | 134.6,-133.3,-43.9,45.2 79 | 134.6,-133.2,-44.1,45.1 80 | 134.6,-133,-44,45.2 81 | 134.6,-132.8,-43.9,45.4 82 | 134.7,-133,-43.7,45.5 83 | 134.8,-133.1,-43.7,45.7 84 | 134.8,-133.3,-43.5,45.8 85 | 134.8,-133.4,-43.4,46 86 | 135,-133.6,-43.4,46.1 87 | 134.9,-133.7,-43.4,46.3 88 | 134.9,-133.5,-43.4,46.2 89 | 134.8,-133.3,-43.4,46.1 90 | 134.8,-133.2,-43.5,45.9 91 | 134.8,-133,-43.5,45.8 92 | 134.5,-132.8,-43.5,45.7 93 | 134.4,-132.7,-43.5,45.5 94 | 134,-132.1,-43.2,45.6 95 | 133.7,-131.6,-43.5,45.9 96 | 133.5,-132.2,-43.2,46.2 97 | 133.2,-132.5,-43.5,46.5 98 | 133.1,-132.8,-43.8,46.8 99 | 133.1,-132.7,-44.1,46.2 100 | 133.4,-133.2,-44.5,45.7 101 | 133.7,-133.6,-44.2,45.2 102 | 133.9,-132.8,-44.6,44.8 103 | 134,-132.7,-44.3,44.3 104 | 134.2,-132.5,-44,44.2 105 | 134.2,-132.7,-43.7,44.9 106 | 134.2,-132.8,-43.6,45.7 107 | 134.2,-132.9,-43.4,46.2 108 | 134.2,-133.1,-43.1,46.6 109 | 134.2,-132.9,-42.9,47.1 110 | 134.2,-132.7,-42.8,47.3 111 | 134.3,-132.6,-42.6,47.1 112 | 134.3,-132.4,-42.6,47 113 | 134.3,-132.3,-42.6,46.8 114 | 134.3,-132.1,-42.7,46.7 115 | 134.3,-132.1,-42.8,46.5 116 | 134.3,-131.9,-43,46.4 117 | 134.4,-132.1,-43.2,46.6 118 | 134.4,-132.1,-43.2,46.7 119 | 134.6,-132.2,-43.2,46.9 120 | 134.7,-132.7,-43,47 121 | 134.8,-132.8,-42.8,46.8 122 | 134.3,-133,-43.1,47.2 123 | 134,-133,-43.4,46.9 124 | 133.9,-133.1,-43.7,46.6 125 | 133.8,-133.3,-43.8,46.1 126 | 133.8,-133.3,-43.8,45.8 127 | 133.9,-133.4,-44.1,45.9 128 | 134.5,-133.7,-43.8,45.7 129 | 134.7,-133.9,-43.4,45.8 130 | 134.8,-134,-42.9,46 131 | 134.8,-133.9,-42.6,46.3 132 | 134.7,-133.9,-42.8,46.4 133 | 134.6,-133.7,-42.6,46.6 134 | 134.6,-133.5,-42.8,46.2 135 | 134.8,-133.4,-42.9,46.4 136 | 135,-133.2,-43,46.5 137 | 135,-133.3,-43.3,46.7 138 | 135,-133.3,-43.3,46.8 139 | 135,-133.4,-43.3,47 140 | 135.1,-133.6,-43.1,47.4 141 | 135,-133.8,-43.3,47.6 142 | 134.9,-133.9,-43.4,47.4 143 | 134.9,-133.9,-43.4,47.3 144 | 134.9,-133.7,-43.5,47.2 145 | 134.9,-133.6,-43.5,47 146 | 134.9,-133.2,-43.3,46.8 147 | 135.2,-132.8,-43.1,46.7 148 | 135.1,-133.1,-43.1,46.8 149 | 134.8,-133.1,-43.4,46.5 150 | 134.5,-133.2,-43.7,46.4 151 | 134.1,-133.4,-44,46.1 152 | 133.8,-133.6,-44.1,45.8 153 | 133.7,-133.7,-44.2,45.7 154 | 133.7,-133.4,-44.4,45.6 155 | 133.8,-133.2,-43.9,45.7 156 | 134,-133.2,-43.7,45.7 157 | 134.2,-133.2,-43.3,45.9 158 | 134.3,-133.2,-43.3,46.1 159 | 134.8,-133.3,-43.1,46.2 160 | 135.1,-133.3,-42.9,46.4 161 | 135.1,-133.5,-43.2,46.5 162 | 135.2,-133.3,-43.1,46.4 163 | 135.1,-133.3,-43.3,46.4 164 | 134.9,-133.3,-43.3,46.5 165 | 134.5,-133.1,-43.3,46.3 166 | 134.2,-132.9,-43.3,46.1 167 | 134.2,-132.8,-43,46 168 | 134,-132.7,-42.8,46.1 169 | 134.2,-132.3,-42.8,46.1 170 | 134.3,-132.3,-42.8,46.1 171 | 134.6,-132.6,-42.8,46.6 172 | 135,-132.9,-42.9,46.8 173 | 134.7,-133.2,-43,46.9 174 | 134.7,-132.6,-43.1,46.6 175 | 134.6,-132.3,-43.1,46.5 176 | 134.4,-132.3,-43.2,46.6 177 | 134.3,-132.3,-43.1,46.3 178 | 134.2,-132.6,-43.1,46.4 179 | 134.6,-132.4,-43.1,46.5 180 | 134.8,-132.9,-43,46.7 181 | 134.8,-133,-42.8,46.7 182 | 134.8,-133,-42.8,46.7 183 | 134.8,-132.9,-42.8,46.6 184 | 134.8,-132.5,-42.8,46.4 185 | 134.6,-132.3,-43,46.3 186 | 134.5,-132.3,-43.1,46.6 187 | 134.6,-132.5,-43.2,46.7 188 | 134.9,-132.5,-43.1,46.9 189 | 135.3,-132.8,-43,46.8 190 | 135.3,-132.6,-42.8,46.6 191 | 135.2,-132.4,-42.6,46.4 192 | 134.9,-132.4,-42.8,46.3 193 | 134.8,-132.8,-43,46.2 194 | 134.7,-132.8,-43.2,46.4 195 | 134.7,-132.8,-43.5,46.4 196 | 134.6,-133.1,-43.5,46.4 197 | 134.4,-133.1,-43.4,46.4 198 | 134.3,-133.1,-43.4,46.4 199 | 134.2,-133.1,-43.4,46.4 200 | 134.4,-133.3,-43.2,46.7 201 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Triangulation Toolbox for MATLAB 2 | _Triangulation Toolbox_ is an open-source project to share algorithms, datasets, and benchmarks for landmark-based localization. It is implemented in [MATLAB][] script language and distributed under [Simplified BSD License][]. 3 | 4 | * Homepage: [http://sites.google.com/site/sunglok/tt](http://sites.google.com/site/sunglok/tt) 5 | 6 | ### Installation 7 | 1. Download the toolbox through [its GitHub ZIP URL](https://github.com/SunglokChoi/Triangulation-Toolbox/archive/master.zip) 8 | 1. Unzip the downloaded file, _Triangulation-Toolbox-master.zip_, on your target directory 9 | 1. Execute `run_test_aux` or `run_test_localize` to check its working in MATLAB 10 | 11 | ### File Description 12 | Use `help` command to know each function in detail, for example, `help observe_distance`. 13 | 14 | * __2D Localization Algorithms__: localize2d\_\*.m 15 | * __3D Localization Algorithms__: localize3d\_\*.m 16 | * __Observation Functions__: [observe_distance.m][], [observe_distance_relative.m][], [observe_bearing.m][], [observe_displacement.m][], [observe_pose.m][] 17 | * __Random Noise Generators__: [apply_noise_gauss.m][] 18 | * __Accuracy Criteria__: [error_position.m][], [error_orientation.m][], [error_reprojection.m][] 19 | * __Utility Functions__ 20 | * Unit-test: [test_is_true.m][], [test_is_near.m][] 21 | * Angular Conversions: [tran_rad2deg.m][], [tran_deg2rad.m][], [tran_rad2rot.m][], [tran_rot2rad.m][], [trim_rad.m][] 22 | * __Scripts for Examples__: [run_example.m][] 23 | * __Scripts for Unit-test__: [run_test_aux.m][], [run_test_localize.m][] 24 | * __Scripts for Performance Evaluation__ 25 | * Benchmark with Random Landmarks (with Map Uncertainty and Measurement Noise): [run_eval_random.m][] 26 | * Benchmark with Roh's Angulation Dataset: [run_eval_roh.m][] 27 | * Benchmark with UTIAS MRCLAM Dataset: [run_eval_mrclam.m][] 28 | * [Position/Orientaion Estimate Distribution Drawing][] for Each Benchmark: [run_draw_distribution.m][] 29 | * __Real Datasets__ 30 | * Roh's Angulation Dataset: [dataset_roh][] 31 | * UTIAS MRCLAM Dataset: [dataset_mrclam][] (converted by [run_conv_mrclam.m][]) 32 | 33 | ### Example 34 | ```matlab 35 | trueMap = ... 36 | [ ... 37 | % x, y, z, r_x, r_y, r_z ... 38 | 0, 0, 0, 0, 0, 0; ... 39 | 8, 0, 0, 0, 0, tran_deg2rad( +90); ... 40 | 8, 8, 0, 0, 0, tran_deg2rad(-180); ... 41 | ]; 42 | truePose = [3, 2, 0, 0, 0, pi / 9]; 43 | obsData = observe_distance(trueMap, truePose); % Simulate observation 44 | estPose = localize2d_sayed05_toa(obsData, trueMap); % Estimate position 45 | ``` 46 | See [run_example.m][] for more complex example with visualization. 47 | 48 | ### Reference 49 | * Sunglok Choi, __Triangulation Toolbox: Open-source Algorithms and Benchmark for Landmark-based Localization__, in Proceedings of IEEE International Conference on Robotics and Automation (ICRA), 2014 50 | 51 | ### Authors 52 | * [Sunglok Choi](http://sites.google.com/site/sunglok/) (sunglok AT hanmail DOT net) 53 | 54 | ### Acknowledgement 55 | The authors thank to the following contributors and projects. 56 | 57 | * [Prof. Federico Thomas](http://www.iri.upc.edu/people/thomas/) shared his trilateration code at his homepage. (See [the algorithm][localize3d_thomas05.m]) 58 | * Hyun-Chul Roh provided his angulation dataset. (See [the dataset][dataset_roh]) 59 | * [Dr. Keith Leung](http://asrl.utias.utoronto.ca/~kykleung), Yoni Halpern, [Prof. Tim Barfoot](http://asrl.utias.utoronto.ca/~tdb), and [Prof. Hugh Liu](http://www.flight.utias.utoronto.ca/fsc/index.php?id=204) shared their MRCLAM dataset. (See [the dataset][dataset_mrclam]) 60 | 61 | [MATLAB]: http://www.mathworks.com/products/matlab/ 62 | [Simplified BSD License]: http://opensource.org/licenses/BSD-2-Clause 63 | 64 | [localize3d_thomas05.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/localize3d_thomas05.m 65 | [observe_distance.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/observe_distance.m 66 | [observe_distance_relative.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/observe_distance_relative.m 67 | [observe_bearing.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/observe_bearing.m 68 | [observe_displacement.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/observe_displacement.m 69 | [observe_pose.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/observe_pose.m 70 | [apply_noise_gauss.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/apply_noise_gauss.m 71 | [error_position.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/error_position.m 72 | [error_orientation.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/error_orientation.m 73 | [error_reprojection.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/error_reprojection.m 74 | [test_is_true.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/test_is_true.m 75 | [test_is_near.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/test_is_near.m 76 | [tran_rad2deg.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/tran_rad2deg.m 77 | [tran_deg2rad.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/tran_deg2rad.m 78 | [tran_rad2rot.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/tran_rad2rot.m 79 | [tran_rot2rad.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/tran_rot2rad.m 80 | [trim_rad.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/trim_rad.m 81 | [run_example.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_example.m 82 | [run_test_aux.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_test_aux.m 83 | [run_test_localize.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_test_localize.m 84 | [run_eval_random.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_eval_random.m 85 | [run_eval_roh.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_eval_roh.m 86 | [run_eval_mrclam.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_eval_mrclam.m 87 | [run_draw_distribution.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_draw_distribution.m 88 | 89 | [dataset_roh]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/dataset_roh 90 | [dataset_mrclam]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/dataset_mrclam 91 | [run_conv_mrclam.m]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/run_conv_mrclam.m 92 | [Position/Orientaion Estimate Distribution Drawing]: https://github.com/SunglokChoi/Triangulation-Toolbox/blob/master/benchmark_result/run_eval_random(map%2C2d)/ex1_06_position.png 93 | -------------------------------------------------------------------------------- /run_conv_mrclam.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== MRCLAM Dataset Conversion for Triangulation Toolbox =='); 5 | 6 | % Configure conversion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | dataset.robot_n = 5; 8 | dataset.dir = 'dataset_mrclam/MRCLAM_Dataset1'; 9 | dataset.output = 'dataset_mrclam/mrclam1.mat'; 10 | outlier.distance = 0.5; % A threshold for outlier distance [m] 11 | outlier.rotation = 0.25; % A threshold for outlier bearing [rad] 12 | threshold.elapse = 1; % The maximum duration [sec] for being stationary state 13 | threshold.distance = 0.01; % The minimum translational change [m] for being stationary state 14 | threshold.rotation = 0.01; % The minimum rotational change [rad] for being stationary state 15 | threshold.landmark = 3; % The minimum number of landmarks 16 | 17 | % Convert MRCLAM dataset to landmark-based localization %%%%%%%%%%%%%%%%%%%%%%% 18 | % Read landmarks and their barcode ID 19 | landmark = textread([dataset.dir, '/Landmark_Groundtruth.dat'], '', 'delimiter', '\t', 'commentstyle', 'shell'); 20 | landmark = landmark(:,1:3); 21 | barcode = textread([dataset.dir, '/Barcodes.dat'], '', 'delimiter', '\t', 'commentstyle', 'shell'); 22 | threshold.minId = min(landmark(:,1)); 23 | threshold.maxId = max(landmark(:,1)); 24 | 25 | % Extract useful measurements for the toolbox 26 | expNum = 0; 27 | groundtruth = []; 28 | measurement = []; 29 | elapsedTime = []; 30 | for r = 1:dataset.robot_n 31 | % 1. Read measurements and their true trajectory 32 | prefix = [dataset.dir, '/Robot', num2str(r), '_']; 33 | truth = textread([prefix, 'Groundtruth.dat'], '', 'delimiter', '\t', 'commentstyle', 'shell'); 34 | measure = textread([prefix, 'Measurement.dat'], '', 'delimiter', '\t', 'commentstyle', 'shell'); 35 | for j = 1:size(barcode,1) 36 | measure(measure(:,2) == barcode(j,2),2) = barcode(j,1); % Substitute 'barcode ID' to 'subject ID' 37 | end 38 | disp([' * Robot #', num2str(r), ': ', num2str(size(measure,1)), ' frames in the measurements.']); 39 | disp([' * Robot #', num2str(r), ': ', num2str(size(truth,1)), ' frames in the ground truth.']); 40 | 41 | % 2. Calculate piecewise translation and rotation 42 | delta.time = [0; truth(2:end,1) - truth(1:end-1,1)]; 43 | delta.dist = truth(2:end,2:3) - truth(1:end-1,2:3); 44 | delta.dist = [0; sqrt(delta.dist(:,1).^2 + delta.dist(:,2).^2)]; 45 | delta.rota = [0; truth(2:end,4) - truth(1:end-1,4)]; 46 | delta.rota(delta.rota >= pi) = delta.rota(delta.rota >= pi) - 2 * pi; 47 | delta.rota(delta.rota < -pi) = delta.rota(delta.rota < -pi) + 2 * pi; 48 | delta.rota = abs(delta.rota); 49 | 50 | % 3. Find instantaneously stationary sections in the ground truth 51 | section.t_index = []; 52 | section.t_time = []; 53 | start = 1; 54 | while start < length(delta.dist) 55 | acc.time = 0; 56 | acc.dist = 0; 57 | acc.rota = 0; 58 | for j = (start+1):length(delta.dist) 59 | acc.time = acc.time + delta.time(j); 60 | acc.dist = acc.dist + delta.dist(j); 61 | acc.rota = acc.rota + delta.rota(j); 62 | if (acc.time > threshold.elapse) || (acc.dist > threshold.distance) || (acc.rota > threshold.rotation) 63 | break; 64 | end 65 | end 66 | if j > (start+1) 67 | section.t_index = [section.t_index; start, j-1]; 68 | section.t_time = [section.t_time; truth(start,1), truth(j-1,1)]; 69 | end 70 | start = start + 1; 71 | end 72 | disp([' * Robot #', num2str(r), ': ', num2str(size(section.t_index,1)), ' subsequences satisfy time, translation, and orientation conditions.']); 73 | 74 | % 4. Match the stationary sections to the measurements 75 | section.m_index = zeros(size(section.t_index)); 76 | start = 1; 77 | for j = 1:size(section.m_index,1) 78 | minIndex = inf; 79 | maxIndex = 0; 80 | for k = start:size(measure,1) 81 | if measure(k,1) > section.t_time(j,2) 82 | break; 83 | elseif measure(k,1) >= section.t_time(j,1) 84 | minIndex = min(minIndex, k); 85 | maxIndex = max(maxIndex, k); 86 | end 87 | end 88 | section.m_index(j,:) = [minIndex, maxIndex]; 89 | if ~isinf(minIndex) 90 | start = minIndex; 91 | end 92 | end 93 | 94 | % 5. Select the stationary sections which statisfy the minimum landmark condition 95 | selected.truth = []; 96 | selected.measure = []; 97 | selected.elapse = []; 98 | for j = 1:size(section.m_index,1) 99 | if ~isinf(section.m_index(j,1)) && (section.m_index(j,2) ~= 0) % If there are landmarks 100 | % 5.1. Reject landmarks from other robots 101 | sample = measure(section.m_index(j,1):section.m_index(j,2),2:end); 102 | sample = sample(sample(:,1) >= threshold.minId,:); 103 | sample = sample(sample(:,1) <= threshold.maxId,:); 104 | 105 | % 5.2. Reject outlier data 106 | if section.t_index(j,1) == section.t_index(j,2) 107 | pose = truth(section.t_index(j,1),2:4); 108 | else 109 | pose = mean(truth(section.t_index(j,1):section.t_index(j,2),2:4)); 110 | end 111 | map = []; 112 | for k = 1:size(sample,1) 113 | map = [map; landmark(landmark(:,1) == sample(k,1), 2:end)]; 114 | end 115 | distance = observe_distance([map, zeros(size(map,1),4)], [pose(1:2), 0, 0, 0, pose(3)]); 116 | bearing = observe_bearing([map, zeros(size(map,1),4)], [pose(1:2), 0, 0, 0, pose(3)]); 117 | diff = abs(sample(:,2:end) - [distance, bearing(:,1)]); 118 | sample = sample((diff(:,1) < outlier.distance) & (diff(:,2) < outlier.rotation),:); 119 | 120 | % 5.3. Remove duplicated measurements (select the most recent one) 121 | table = zeros(threshold.maxId,size(sample,2)); 122 | table(sample(:,1),:) = sample; 123 | mask = zeros(threshold.maxId,1); 124 | mask(sample(:,1)) = 1; 125 | sample = table(mask == 1,:); 126 | 127 | % 5.4. Accept a section which statisfies the minimum landmark condition 128 | if sum(mask) >= threshold.landmark 129 | expNum = expNum + 1; 130 | selected.truth = [selected.truth; expNum, pose]; 131 | selected.measure = [selected.measure; expNum * ones(size(sample,1),1), sample]; 132 | selected.elapse = [selected.elapse; expNum, section.t_time(j,2) - section.t_time(j,1)]; 133 | end 134 | end 135 | end 136 | disp([' * Robot #', num2str(r), ': ', num2str(size(selected.truth,1)), ' subsequences satisfy the minimal landmark condition.']); 137 | 138 | groundtruth = [groundtruth; selected.truth]; 139 | measurement = [measurement; selected.measure]; 140 | elapsedTime = [elapsedTime; selected.elapse]; 141 | end 142 | disp([' * ', num2str(size(groundtruth,1)), ' subsequences are extracted.']); 143 | 144 | % Save landmarks, ground truths, measurements 145 | landmark(:,1) = landmark(:,1) - threshold.minId + 1; % Start index from 1 146 | measurement(:,2) = measurement(:,2) - threshold.minId + 1; % Start index from 1 147 | save(dataset.output, 'landmark', 'groundtruth', 'measurement'); 148 | -------------------------------------------------------------------------------- /run_eval_random.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | disp('== Localization Evaluatioin (with Random Landmarks) for Triangulation Toolbox =='); 5 | 6 | % Configure experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | config.dim = 2; % Dimension of localization (2 or 3) 8 | config.observe = []; % An observation function to apply noise 9 | % (e.g. @observe_bearing / c.f. empty []: noise is applied to the map) 10 | config.space = [100, 100, 50]; % Size of the operating space 11 | config.pool = 10000; % The number of pre-generated landmarks (> 5) 12 | config.trial = 2000; % The number of trials (> 1) 13 | config.pose = [50, 50, 0, 0, 0, pi/4]; % Pose of the target object 14 | config.fixNoise = 0.1; % Standard deviation of noise (default) 15 | config.fixN = 4; % The number of landmarks for localization (default) 16 | config.varNoise = 0:0.1:1; % Range of std. of noise 17 | config.varN = [2, 3, 4, 5, 8, 16, 32, 64, 128]; % Range of the number of landmarks for localization 18 | config.algorithm = ... % Description of localization algorithms 19 | { ... 20 | % #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ... 21 | 1, 2, 'Sayed05-TOA2D',@localize2d_sayed05_toa, @observe_distance, 3, [1 1 0 0 0 0], 'kx:'; ... 22 | 2, 2, 'Sayed05-TDOA', @localize2d_sayed05_tdoa, @observe_distance_relative,3, [1 1 0 0 0 0], 'k--'; ... 23 | 3, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ... 24 | 4, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ... 25 | 5, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ... 26 | 6, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ... 27 | 7, 2, 'Sayed05-AOA', @localize2d_sayed05_aoa, @observe_displacement, 2, [1 1 0 0 0 1], 'ko-'; ... 28 | 8, 2, 'Pose', @localize2d_pose, @observe_pose, 1, [1 1 0 0 0 1], 'r--'; ... 29 | 9, 3, 'Sayed05-TOA3D',@localize3d_sayed05_toa, @observe_distance, 4, [1 1 1 0 0 0], 'kx:'; ... 30 | 10, 3, 'Thomas05', @localize3d_thomas05, @observe_distance, 3, [1 1 1 0 0 0], 'm+-'; ... 31 | }; 32 | config.verbose = true; % Show progress of experiments (true or false) 33 | config.warning = 'off'; % Show warning during experiments ('on' or 'off') 34 | config.matLoad = false; % Use saved MAT-file without experiments (true or false) 35 | config.matFile = 'run_eval_random.mat'; % Filename for loading and saving MAT-file 36 | config.csvFile = 'run_eval_random.csv'; % Filename for writing CSV-file 37 | 38 | variable.name = {'Magnitude of Noise [m]', 'Number of Landmarks'}; % Name of independent variables 39 | variable.value = {config.varNoise, config.varN}; % Range of independent variables 40 | variable.repr = {config.varNoise, config.varN}; % Range of independent variables (in graphs and table) 41 | variable.format = {'%.1f', '%d'}; % Format for printing text 42 | 43 | criteria.name = {'Position Error [m]', 'Orientation Error [deg]', ... 44 | 'Computing Time [msec]', 'Number of Failures'}; % Name of evaluation criteria 45 | criteria.repr = {@median, @median, @median, @sum}; % Functions for calculating representive values 46 | % (e.g. mean, median, std, and sum) 47 | criteria.format = {'%.6f', '%.3f', '%.6f', '%d'}; % Format for printing text 48 | 49 | % Perform experiments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | config.algoDims = 2; 51 | config.algoName = 3; 52 | config.algoEstm = 4; 53 | config.algoObsv = 5; 54 | config.algoMinN = 6; 55 | config.algoVald = 7; 56 | config.algoLine = 8; 57 | config.algoSelM = 1:size(config.algorithm,1); 58 | choose = [config.algorithm{:,config.algoDims}] == config.dim; 59 | if ~isempty(config.observe) 60 | for m = 1:length(choose) 61 | if choose(m) == 1 && ~isequal(config.algorithm{m,config.algoObsv}, config.observe) 62 | choose(m) = 0; 63 | end 64 | end 65 | end 66 | config.algoSelM = config.algoSelM(choose); 67 | if ~config.matLoad 68 | % 1. Generate features randomly 69 | if config.pool <= 5 70 | error('TT: The number of landmarks ''config.pool'' should be more than 5!'); 71 | end 72 | pool = zeros(config.pool,6); 73 | pool(:,1:config.dim) = repmat(config.space(1:config.dim), config.pool, 1) .* rand(config.pool,config.dim); 74 | pool(:,4:6) = 2 * pi * rand(config.pool,3) - pi; 75 | 76 | % 2. Execute each algorithm 'config.trial' times 77 | if config.trial <= 1 78 | error('TT: The number of trials ''config.trial'' should be more than 1!'); 79 | end 80 | for ex = 1:length(variable.value) 81 | for v = 1:length(variable.value{ex}) 82 | record.perf{ex,v} = inf * ones(config.trial, length(criteria.name), size(config.algorithm,1)); 83 | record.pose{ex,v} = zeros(config.trial, 6, size(config.algorithm,1)); 84 | end 85 | end 86 | if isequal(config.warning, 'off'); 87 | warning off; 88 | end 89 | for ex = 1:length(variable.value) % Loop for 'ex'periments 90 | if config.verbose 91 | fprintf('\n==== Progress on Experiment #%d: %s ====\n', ex, variable.name{ex}); 92 | end 93 | for v = 1:length(variable.value{ex}) % Loop for 'v'ariables 94 | param = [config.fixNoise, config.fixN]; 95 | param(ex) = variable.value{ex}(v); 96 | for t = 1:config.trial % Loop for 't'rials 97 | % Select landmarks randomly 98 | sample = zeros(1,config.pool,'uint8'); 99 | while sum(sample) < param(2) 100 | index = floor(config.pool * rand()) + 1; 101 | sample(index) = 1; 102 | end 103 | cleanMap = pool(sample == 1,:); 104 | noisyMap = cleanMap; 105 | if isempty(config.observe) 106 | noisyMap(:,1:config.dim) = apply_noise_gauss(cleanMap(:,1:config.dim), param(1)); 107 | end 108 | 109 | for m = config.algoSelM % Loop for 'm'ethods 110 | % Check the operating condition 111 | if (config.dim > config.algorithm{m,config.algoDims}) || ... 112 | (param(2) < config.algorithm{m,config.algoMinN}) 113 | continue; 114 | end 115 | 116 | % Estimate pose 117 | obsData = feval(config.algorithm{m,config.algoObsv}, cleanMap, config.pose); 118 | if isequal(config.algorithm{m,config.algoObsv}, config.observe) 119 | obsData = apply_noise_gauss(obsData, param(1)); 120 | end 121 | tic; 122 | [pose, valid] = feval(config.algorithm{m,config.algoEstm}, obsData, noisyMap); 123 | elapse = toc * 1000; % [sec] to [msec] 124 | if size(pose,1) > 1 % When there are multiple solutions 125 | bestIndex = 1; 126 | bestError = inf; 127 | for i = 1:size(pose,1) 128 | err = norm(config.pose - pose(i,:)); 129 | if bestError > err 130 | bestIndex = i; 131 | bestError = err; 132 | end 133 | end 134 | pose = pose(bestIndex,:); 135 | valid = valid(bestIndex,:); 136 | end 137 | record.perf{ex,v}(t,1,m) = error_position(config.pose(1:3), pose(1:3)); 138 | if valid(end) 139 | record.perf{ex,v}(t,2,m) = tran_rad2deg(error_orientation(config.pose(4:6), pose(4:6))); % [rad] to [deg] 140 | end 141 | record.perf{ex,v}(t,3,m) = elapse; 142 | record.perf{ex,v}(t,4,m) = ~isequal(valid, config.algorithm{m,config.algoVald}); 143 | record.pose{ex,v}(t,:,m) = pose; 144 | end 145 | end % End of 'for t' 146 | 147 | % Print progress 148 | if config.verbose 149 | fprintf('Each method performed %d times. [Param] %s\n', config.trial, num2str(param)); 150 | end 151 | end % End of 'for v' 152 | end % End of 'for ex' 153 | warning on; 154 | if ~isempty(config.matFile) 155 | save(config.matFile, 'config', 'variable', 'criteria', 'record'); 156 | end 157 | else 158 | backup.matFile = config.matFile; 159 | backup.csvFile = config.csvFile; 160 | load(config.matFile); 161 | config.matFile = backup.matFile; 162 | config.csvFile = backup.csvFile; 163 | end % End of 'if config.matLoad' 164 | 165 | % Show experimental results %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 166 | % 3. Retrieve 'result' from 'record' 167 | for ex = 1:length(variable.value) 168 | for cr = 1:length(criteria.name) 169 | result{ex,cr} = zeros(size(config.algorithm,1), length(variable.value{ex})); 170 | end 171 | end 172 | for ex = 1:length(variable.value) 173 | for cr = 1:length(criteria.name) 174 | for v = 1:length(variable.value{ex}) 175 | for m = config.algoSelM 176 | result{ex,cr}(m,v) = feval(criteria.repr{cr}, record.perf{ex,v}(:,cr,m)); 177 | end 178 | end 179 | end 180 | end 181 | 182 | % 4. Write 'result' to a text file 183 | if ~isempty(config.csvFile) 184 | fid = fopen(config.csvFile, 'wt'); 185 | if fid < 0 186 | error('TT: Cannot open a file, ''%s''!', config.csvFile); 187 | end 188 | for ex = 1:length(variable.value) 189 | fprintf(fid, '==== Results on Experiment #%d: %s ====\n', ex, variable.name{ex}); 190 | for cr = 1:length(criteria.name) 191 | % Print header 192 | fprintf(fid, '\n%d) %s\n', cr, criteria.name{cr}); 193 | fprintf(fid, '%s', variable.name{ex}); 194 | for v = 1:length(variable.value{ex}) 195 | fprintf(fid, ', %.1f', variable.repr{ex}(v)); 196 | end 197 | fprintf(fid, '\n'); 198 | 199 | % Print results of each method 200 | for m = config.algoSelM 201 | fprintf(fid, '%s', config.algorithm{m,config.algoName}); 202 | for v = 1:length(variable.value{ex}) 203 | fprintf(fid, [', ', criteria.format{cr}], result{ex,cr}(m,v)); 204 | end 205 | fprintf(fid, '\n'); 206 | end 207 | end 208 | fprintf(fid, '\n'); 209 | end 210 | fclose(fid); 211 | end 212 | 213 | % 5. Show 'result' as graphs 214 | for ex = 1:length(variable.value) 215 | for cr = 1:length(criteria.name) 216 | % Draw results of each method 217 | isDrawn = []; 218 | figure('Color', [1, 1, 1]); 219 | hold on; 220 | set(gca, 'FontSize', 12); 221 | box on; 222 | grid on; 223 | for m = config.algoSelM 224 | if (cr == 2) && (config.algorithm{m,config.algoVald}(end) == 0), continue; end 225 | plot(variable.repr{ex}, result{ex,cr}(m,:), ... 226 | config.algorithm{m,config.algoLine}, 'LineWidth', 2, 'MarkerSize', 10); 227 | isDrawn = [isDrawn, m]; 228 | end 229 | if ex == 2 230 | set(gca, 'XScale', 'log'); 231 | set(gca, 'XTick', [2, 4, 8, 16, 32, 64, 128]); 232 | set(gca, 'XLim', [2, 128]); 233 | end 234 | %title(sprintf('Experiment #%d: %s - %s', ex, variable.name{ex}, criteria.name{cr}), 'FontSize', 12); 235 | xlabel(variable.name{ex}, 'FontSize', 12); 236 | ylabel(criteria.name{cr}, 'FontSize', 12); 237 | legend(config.algorithm(isDrawn,config.algoName), 'FontSize', 12); 238 | hold off; 239 | end 240 | end 241 | --------------------------------------------------------------------------------