├── README.md ├── SEn_SOnp1.m ├── SEn_point_cloud_reg.m ├── SOnp1_SEn.m ├── garage3.jpg └── orthonormalize.m /README.md: -------------------------------------------------------------------------------- 1 | # SE(n)++ for Pose Estimation 2 | orthonormalize.m - Orthonormalization of square matrices 3 | 4 | SEn_SOnp1.m - Transformation from SE(n) to SO(n + 1) 5 | 6 | SOnp1_SEn.m - Transformation from SO(n + 1) to SE(n) 7 | 8 | SEn_point_cloud_reg.m - An example for point cloud registration using SE(n)++ 9 | 10 | 11 | ![](https://github.com/zarathustr/SEnpp/blob/master/garage3.jpg) 12 | 13 | # Reference 14 | Wu, J., Liu, M. et al. (2020) SE(n)++: An Efficient Solution to Multiple Pose Estimation Problems. IEEE Transactions on Cybernetics. 15 | -------------------------------------------------------------------------------- /SEn_SOnp1.m: -------------------------------------------------------------------------------- 1 | % The mapping from SE(n) to SO(n + 1) 2 | % 3 | % Author: Jin Wu 4 | % e-mail: jin_wu_uestc@hotmail.com 5 | % website: www.jinwu.science 6 | % www.ram-lab.com 7 | % 8 | % References: 9 | % 10 | % [1] Wu, J., Liu, M. (2019) 11 | % Simultaneous SO(n) Solutions to Hand-eye Calibration Problems, 12 | % IEEE Transactions on Automation Science and Engineering 13 | % 14 | % [2] Wu, J., Sun, Y., Wang, M., Liu, M. (2019) 15 | % Hand-eye Calibration: 4D Procrustes Analysis Approach, 16 | % IEEE Transactions on Instrumentation and Measurement 17 | 18 | 19 | function SA = SEn_SOnp1(A, dd) 20 | s = size(A); 21 | dim = s(1); 22 | RA = A(1 : dim - 1, 1 : dim - 1); 23 | tA = A(1 : dim - 1, dim); 24 | SA = [RA, tA / dd; 25 | - tA' * RA / dd, 1]; 26 | end -------------------------------------------------------------------------------- /SEn_point_cloud_reg.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | len = 100000; 6 | dim = 10; 7 | d = 1e6; 8 | num = 1; 9 | err = 1e-4; 10 | 11 | ll = [0, 0]; 12 | 13 | for j = 1 : num 14 | b = zeros(dim, len); 15 | r = randn(dim, len); 16 | T = randn(dim, 1); 17 | 18 | B = randn(dim, dim); 19 | R = orthonormalize(B); 20 | 21 | for i = 1 : len 22 | b(:, i) = R * r(:, i) + T + err * randn(dim, 1); 23 | end 24 | 25 | 26 | M = zeros(dim + 1, dim + 1); 27 | for i = 1 : len 28 | b_ = b(:, i); 29 | r_ = r(:, i); 30 | 31 | b_aug = [b_; d]; 32 | r_aug = [r_; d]; 33 | 34 | b_aug = b_aug ./ norm(b_aug); 35 | r_aug = r_aug ./ norm(r_aug); 36 | 37 | M = M + 1 / len * b_aug * r_aug'; 38 | end 39 | 40 | [RR, t] = SOnp1_SEn(orthonormalize(M), d); 41 | 42 | 43 | L = 0; 44 | for i = 1 : len 45 | L = L + 1 / len * norm(b(:, i) - RR * r(:, i) - t)^2; 46 | end 47 | 48 | 49 | mean_b = zeros(dim, 1); 50 | mean_r = zeros(dim, 1); 51 | for i = 1 : len 52 | mean_b = mean_b + 1 / len * b(:, i); 53 | mean_r = mean_r + 1 / len * r(:, i); 54 | end 55 | 56 | M = zeros(dim, dim); 57 | for i = 1 : len 58 | b_ = b(:, i) - mean_b; 59 | r_ = r(:, i) - mean_b; 60 | 61 | M = M + 1 / len * b_ * r_'; 62 | end 63 | R_ = orthonormalize(M); 64 | t_ = mean_b - R_ * mean_r; 65 | 66 | 67 | LL = 0; 68 | for i = 1 : len 69 | LL = LL + 1 / len * norm(b(:, i) - R_ * r(:, i) - t_)^2; 70 | end 71 | 72 | 73 | ll = ll + 1 / num * [L, LL]; 74 | end 75 | 76 | ll 77 | -------------------------------------------------------------------------------- /SOnp1_SEn.m: -------------------------------------------------------------------------------- 1 | % The mapping from SO(n + 1) to SE(n) 2 | % 3 | % Author: Jin Wu 4 | % e-mail: jin_wu_uestc@hotmail.com 5 | % website: www.jinwu.science 6 | % www.ram-lab.com 7 | % 8 | % References: 9 | % 10 | % [1] Wu, J., Liu, M. (2019) 11 | % Simultaneous SO(n) Solutions to Hand-eye Calibration Problems, 12 | % IEEE Transactions on Automation Science and Engineering 13 | % 14 | % [2] Wu, J., Sun, Y., Wang, M., Liu, M. (2019) 15 | % Hand-eye Calibration: 4D Procrustes Analysis Approach, 16 | % IEEE Transactions on Instrumentation and Measurement 17 | 18 | 19 | 20 | 21 | function [R, t, X] = SOnp1_SEn(A, d) 22 | s = size(A); 23 | dim = s(1); 24 | 25 | t = A(1 : dim - 1, dim) * d; 26 | R = A(1 : dim - 1, 1 : dim - 1); 27 | X = [R, t; 28 | zeros(1, dim - 1), 1]; 29 | 30 | end -------------------------------------------------------------------------------- /garage3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/SEnpp/3d5d55aefaeaedbf023f32ae303fb20370a86cb2/garage3.jpg -------------------------------------------------------------------------------- /orthonormalize.m: -------------------------------------------------------------------------------- 1 | function R = orthonormalize(A) 2 | ss = size(A); 3 | dim = ss(1); 4 | [u, ~, v] = svd(A); 5 | s = eye(dim, dim); 6 | s(dim, dim) = det(u * v); 7 | R = u * s * v'; 8 | end --------------------------------------------------------------------------------