├── figures ├── demo1.jpg ├── demo2.jpg ├── demo3.jpg ├── demo4.jpg ├── demo5.jpg └── demo6.jpg ├── src ├── showPoints.m ├── showSuperellipse.m ├── uniformSampledSuperellipse.m └── EMS2D.m ├── test_script.m ├── LICENSE └── README.md /figures/demo1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo1.jpg -------------------------------------------------------------------------------- /figures/demo2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo2.jpg -------------------------------------------------------------------------------- /figures/demo3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo3.jpg -------------------------------------------------------------------------------- /figures/demo4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo4.jpg -------------------------------------------------------------------------------- /figures/demo5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo5.jpg -------------------------------------------------------------------------------- /figures/demo6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bmlklwx/Robust-superellipse-fitting/HEAD/figures/demo6.jpg -------------------------------------------------------------------------------- /src/showPoints.m: -------------------------------------------------------------------------------- 1 | function [] = showPoints(point, varargin) 2 | 3 | color = 'r'; 4 | MarkerSize = 10; 5 | ShowAxis = 1; 6 | 7 | for i = 1 : size(varargin, 2) 8 | if strcmp(varargin{i}, 'Color') 9 | color = varargin{i + 1}; 10 | end 11 | if strcmp(varargin{i}, 'MarkerSize') 12 | MarkerSize = varargin{i + 1}; 13 | end 14 | if strcmp(varargin{i}, 'ShowAxis') 15 | ShowAxis= varargin{i + 1}; 16 | end 17 | end 18 | 19 | plot(point(1, :), point(2, :), '.', 'Color', color, 'MarkerSize', MarkerSize) 20 | axis equal 21 | 22 | if ShowAxis == 0 23 | axis off 24 | end 25 | hold off 26 | 27 | end -------------------------------------------------------------------------------- /src/showSuperellipse.m: -------------------------------------------------------------------------------- 1 | function [] = showSuperellipse(x, varargin) 2 | color = 'r'; 3 | arclength = 0.1; 4 | ShowAxis = 1; 5 | 6 | for k = 1 : size(varargin, 2) 7 | if strcmp(varargin{k}, 'Color') 8 | color = varargin{k + 1}; 9 | end 10 | if strcmp(varargin{k}, 'ShowAxis') 11 | ShowAxis= varargin{k + 1}; 12 | end 13 | if strcmp(varargin{k}, 'Arclength') 14 | arclength= varargin{k + 1}; 15 | end 16 | end 17 | x(1) = max(x(1), 0.007); 18 | [point] = uniformSampledSuperellipse(x, arclength, 0); 19 | point(:, end + 1) = point(:, 1); 20 | plot(point(1, :), point(2, :), 'Color', color) 21 | axis equal 22 | if ShowAxis == 0 23 | axis off 24 | end 25 | 26 | end -------------------------------------------------------------------------------- /test_script.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear 3 | addpath('./src') 4 | 5 | % random superellipse 6 | x_gt = [max(rand * 2, 0.01), ... 7 | (rand - 0.5) * 1 + 2, (rand - 0.5) * 1 + 2, ... 8 | rand * 2 * pi, (rand - 0.5) * 2, (rand - 0.5) * 2]; 9 | 10 | points = uniformSampledSuperellipse(x_gt, 0.2, 0); 11 | %-------------partial points---------------------- 12 | partial_ratio = 0.6; % keep 60% of the points 13 | k = floor(partial_ratio * size(points, 2)); 14 | idx = randi(size(points, 2)); 15 | distance = vecnorm(points - points(:, idx)); 16 | [~, idx_k] = maxk(distance, k); 17 | points = points(:, idx_k); 18 | %------------------------------------------------- 19 | 20 | num_point = size(points, 2); 21 | %-------------add outliers 1 = 100%--------------- 22 | outlier_ratio = 1; 23 | num_out = round(outlier_ratio * num_point); 24 | sigma = mean(eig((points - mean(points, 2)) * (points - mean(points, 2))'/num_point)); 25 | outlier = mvnrnd(mean(points, 2)', 2 * sigma * eye(2), num_out)'; 26 | points = [points, outlier]; 27 | 28 | %--------------add noise-------------------------- 29 | noise = mvnrnd([0 0], 0.003 * eye(2), size(points, 2))'; 30 | points = points + noise; 31 | 32 | tic 33 | x = EMS2D(points, 'OutlierRatio', 0.9, 'DebugPlot', false); 34 | toc 35 | 36 | disp('Ground truth is ') 37 | disp(x_gt) 38 | disp('Fitting result is ') 39 | disp(x) 40 | 41 | figure(1) 42 | showPoints(points) 43 | hold on 44 | showSuperellipse(x, 'Color', 'g') 45 | hold off -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Weixiao Liu 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A Robust Superellipse Fitting Algorithm 2 | 3 | A 2D version of the EMS algorithm: [CVPR 2022] Robust and Accurate Superquadric Recovery: a Probabilistic Approach. 4 | > [**Robust and Accurate Superquadric Recovery: a Probabilistic Approach**](https://arxiv.org/abs/2111.14517 "ArXiv version of the paper.") 5 | > Weixiao Liu, Yuwei Wu, [Sipu Ruan](https://ruansp.github.io/), [Gregory S. Chirikjian](https://cde.nus.edu.sg/me/staff/chirikjian-gregory-s/) 6 | 7 | Our [original work](https://github.com/bmlklwx/EMS-superquadric_fitting) is to fit [superquadrics](https://en.wikipedia.org/wiki/Superellipsoid) (3D generalization of superellipse) to point clouds. 8 | This is a simple variant to the original paper to solve [superellipse](https://en.wikipedia.org/wiki/Superellipse) (also know as Lamé curve) fitting problem in 2D cases. 9 | The demo (test_script.m) shows the fitting results to randomly generated superellipse-shaped point clouds, with large amount of noise and outliers. 10 | This repo also contains MATLAB functions to sample points almost uniformly on the side of superellipse, and to draw superellipse. 11 | 12 | superquadrics1superquadrics2superquadrics3 13 | 14 | superquadrics4superquadrics5superquadrics6 15 | 16 | For visitors interested in more complex 3D superquadrics fitting, please visit this [repository](https://github.com/bmlklwx/EMS-superquadric_fitting). 17 | 18 | If you find this repo useful, please cite 19 | 20 | > W. Liu, Y. Wu, S. Ruan and G. S. Chirikjian, "Robust and Accurate Superquadric Recovery: a Probabilistic Approach,"
21 | > 2022 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), New Orleans, LA, USA, 2022, pp. 2666-2675,
22 | > doi: 10.1109/CVPR52688.2022.00270. 23 | -------------------------------------------------------------------------------- /src/uniformSampledSuperellipse.m: -------------------------------------------------------------------------------- 1 | function [point] = uniformSampledSuperellipse(para, arclength ,disp) 2 | 3 | sigma = para(1); 4 | scale = para(2 : 3); 5 | xform = para(4 : 6); 6 | 7 | threshold = 1e-2; 8 | num_limit = 10000; 9 | theta = zeros(1, num_limit); 10 | seg = zeros(1, num_limit); 11 | theta(1) = 0; 12 | seg(1) = 1; 13 | for m = 2 : num_limit 14 | [dt, seg_temp] = dtheta(theta(m - 1), arclength, threshold, scale, sigma); 15 | theta_temp = theta(m - 1) + dt; 16 | 17 | if theta_temp > pi/4 18 | theta(m) = pi/4; 19 | break 20 | else 21 | if m < num_limit 22 | theta(m) = theta_temp; 23 | seg(m) = seg_temp; 24 | else 25 | error(['The number of the sampled points exceeds the limit of ', ... 26 | num2str(num_limit * 4),... 27 | '. Please increase the arclength or raise the limit']) 28 | end 29 | end 30 | end 31 | critical = m + 1; 32 | seg(critical) = 1; 33 | for n = critical + 1 : num_limit 34 | [dt, seg_temp] = dtheta(theta(n - 1), arclength, threshold, flip(scale), sigma); 35 | theta_temp = theta(n - 1) + dt; 36 | 37 | if theta_temp > pi/4 38 | break 39 | else 40 | if n < num_limit 41 | theta(n) = theta_temp; 42 | seg(n) = seg_temp; 43 | else 44 | error(['The number of the sampled points exceeds the limit of ', ... 45 | num2str(num_limit * 4),... 46 | '. Please increase the arclength or raise the limit']) 47 | end 48 | end 49 | end 50 | 51 | num_point = n - 1; 52 | theta = theta(1 : num_point); 53 | seg = seg(1 : num_point); 54 | seg = [seg(1 : critical - 1), flip(seg(critical : end))]; 55 | 56 | points_fw = angle2points(theta(1 : critical - 1), scale, sigma); 57 | points_bw = flip(angle2points(theta(critical : end), flip(scale), sigma), 2); 58 | point = [points_fw, [points_bw(2, :); points_bw(1, :)]]; 59 | 60 | point = [point, flip([-point(1, 1 : num_point - 1); point(2, 1 : num_point - 1)], 2), ... 61 | [-point(1, 2 : end); -point(2, 2 : end)], flip([point(1, 2 : num_point - 1); ... 62 | -point(2, 2 : num_point - 1)], 2)]; 63 | 64 | seg = [seg, seg(1 : num_point - 1), seg(2 : end), seg(2 : num_point - 1)]; 65 | 66 | point = [cos(xform(1)), -sin(xform(1)); sin(xform(1)), cos(xform(1))]... 67 | * point + [xform(2); xform(3)]; 68 | 69 | if disp == 1 70 | figure 71 | plot(point(1, seg == 1), point(2, seg == 1), '*') 72 | hold on 73 | plot(point(1, seg == 2), point(2, seg == 2), '*') 74 | hold off 75 | axis equal 76 | end 77 | 78 | %------------------ calculation of theta interval----- -------------------- 79 | function [dt, seg] = dtheta(theta, arclength, threshold, scale, sigma) 80 | if theta < threshold 81 | dt = abs((arclength / scale(2) + (theta)^(sigma))^(1 / sigma) ... 82 | - (theta)); 83 | seg = 1; 84 | else 85 | dt = arclength / sigma * ((cos(theta) ^ 2 * sin(theta) ^ 2) / ... 86 | (scale(1) ^ 2 * cos(theta) ^ (2 * sigma) * sin(theta) ^ 4 + ... 87 | scale(2) ^ 2 * sin(theta) ^ (2 * sigma) * cos(theta) ^ 4))^(1 / 2); 88 | seg = 2; 89 | end 90 | end 91 | %------------------ mapping from angles to points ------------------------- 92 | function [point] = angle2points(theta, scale, sigma) 93 | point = zeros(2, size(theta, 2)); 94 | point(1, :) = scale(1) .* sign(cos(theta)) .* abs(cos(theta)).^sigma; 95 | point(2, :) = scale(2) .* sign(sin(theta)) .* abs(sin(theta)).^sigma; 96 | end 97 | 98 | end -------------------------------------------------------------------------------- /src/EMS2D.m: -------------------------------------------------------------------------------- 1 | function [x, p] = EMS2D(point, varargin) 2 | 3 | % Written by Weixiao Liu @ JHU, NUS 4 | % Initialized on Feb 24th, 2023, Singapore 5 | % ------------------------------------------------------------------------- 6 | % DESCRIPTION: This algorithm solves for the optimal superellipse fitting of a given 7 | % point cloud. Probabilistic model is adpot to formulate the problem, and 8 | % thus is roubust enough to tolerate some amount of outliers. The outlier 9 | % probability is treated as a hidden random variable and is updated via 10 | % the Bayes' rule (EM algorithm). The parameters of the superellipse is 11 | % solved iteratively via maximum likelihood estimation. 12 | % 13 | % INPUT: point - point cloud array (3 x N) 14 | % varargin(optional): 15 | % 'OutlierRatio' - prior outlier probability [0, 1) (default: 0.8) 16 | % we recommend 0 when dealing with clean point cloud. 17 | % 'MaxIterationEM' - maximum number of EM iterations (default: 30) 18 | % 'ToleranceEM' - absolute tolerance of EM (default: 1e-5) 19 | % 'RelativeToleranceEM' - relative tolerance of EM (default: 1e-1) 20 | % 'MaxOptiIterations' - maximum number of optimization iterations per M (default: 10) 21 | % 'Sigma' - initial sigma^2 (default: 0 - auto generate) 22 | % 'MaxSwitch' - maximum number of switches allowed (default: 5) 23 | % 'AdaptiveUpperBound' - introduce adaptive upper bound to restrict the volume of SQ (default: false) 24 | % 'Rescale' - normalize the input point cloud (default: false) 25 | % 26 | % OUTPUT: x - fitted superellipse parameters 27 | % p - outlier probability of the corresponding points 28 | % ------------------------------------------------------------------------- 29 | %% Configuration 30 | 31 | % parsing varagin 32 | [para] = parseInputArgs(point, varargin{:}); 33 | 34 | % set EMS parameters 35 | w = para.OutlierRatio; 36 | iterEM_max = para.MaxIterationEM; 37 | iterEM_min = 3; 38 | toleranceEM = para.ToleranceEM; 39 | relative_toleranceEM = para.RelativeToleranceEM; 40 | adaptive_upper = para.AdaptiveUpperBound; 41 | 42 | % optimization settings 43 | options = optimoptions('lsqnonlin', 'Algorithm', 'trust-region-reflective', 'Display', 'off', 'MaxIterations', para.MaxOptiIterations); 44 | 45 | %% Initialization 46 | 47 | % translate the coordinate to the center of mass 48 | point = double(point); 49 | t0 = mean(point, 2); 50 | point = point - t0; 51 | 52 | % rescale 53 | if para.Rescale == 1 54 | max_length = max(max(point)); 55 | scale = max_length / 10; 56 | point = point / scale; 57 | end 58 | 59 | % eigen analysis (principal component analysis) for initializing rotation 60 | [EigenVector, ~] = EigenAnalysis(point); 61 | if det(EigenVector) ~= 1 62 | EigenVector = [EigenVector(:, 2), EigenVector(:, 1)]; 63 | end 64 | euler0 = atan2(EigenVector(2,1), EigenVector(1,1)); 65 | 66 | % initialize scale as median along transformed axis 67 | point_rot0 = EigenVector' * point; 68 | s0 = [median(abs(point_rot0(1, :))), median(abs(point_rot0(2, :)))] * 1; 69 | 70 | % initial configuration 71 | x0 = [1, reshape(s0, [1, 2]), euler0, zeros(1, 2)]; 72 | 73 | %------------------------------------------- 74 | if para.DebugPlot 75 | figure(1) 76 | showPoints(point) 77 | hold on 78 | showSuperellipse(x0, 'Color', 'g') 79 | hold off 80 | title('Init') 81 | disp('Started in debug mode, please enter in the command window to continue!') 82 | pause 83 | end 84 | %------------------------------------------- 85 | 86 | 87 | % set lower and upper bounds for the superquadrics 88 | upper = 4 * max(max(abs(point))); 89 | lb = [0.0 0.001 0.001 -2*pi -ones(1, 2) * upper]; 90 | ub = [2.0 ones(1, 2) * upper 2*pi ones(1, 2) * upper]; 91 | 92 | %% EMS Algorithm 93 | 94 | % set bounding volume of outlier space 95 | V = (max(point_rot0(1, :)) - min(point_rot0(1, :))) * (max(point_rot0(2, :)) ... 96 | - min(point_rot0(2, :))); 97 | 98 | % outlier probability density 99 | p0 = 1 / V; 100 | 101 | % initialize variance for gaussian model 102 | if para.Sigma == 0 103 | sigma2 = V ^ (1 / 2) / 1; 104 | else 105 | sigma2 = para.Sigma; 106 | end 107 | 108 | % initialize parameters 109 | x = x0; 110 | cost = 0; 111 | switched = 0; 112 | p = ones(1, size(point, 2)); 113 | 114 | for iterEM = 1 : iterEM_max 115 | 116 | % evaluating distance 117 | [dist] = distance(point, x); 118 | 119 | % evaluating corespondence probability 120 | if w ~= 0 121 | p = correspendence(dist, sigma2, w, p0); 122 | end 123 | 124 | % adaptive upper bound 125 | if adaptive_upper == true 126 | R_current = angle2rotm(x(4)); 127 | point_rot_current = R_current' * point - R_current' * x(5 : 6)'; 128 | ub_a = 1.1 * [max(abs(point_rot_current(1, :))), ... 129 | max(abs(point_rot_current(2, :)))]; 130 | ub = [2.0 ub_a 2 * pi ub_a]; 131 | lb = [0.001 0.01 0.01 -2*pi -ub_a]; 132 | end 133 | 134 | % optimization 135 | cost_func = @(x) weighted_dist(x, point, p); 136 | [x_n, cost_n] = lsqnonlin(cost_func, x, lb, ub, options); 137 | 138 | % update sigma 139 | sigma2_n = cost_n / (2 * sum(p)); 140 | 141 | % evaluate relative cost decrease 142 | relative_cost = (cost - cost_n) / cost_n; 143 | if para.DebugPlot 144 | disp('--------------------------------------------------') 145 | disp(['relative_cost: ', num2str(relative_cost)]) 146 | disp(['cost: ', num2str(cost)]) 147 | disp(['cost_n: ', num2str(cost_n)]) 148 | end 149 | %------------------------------------------- 150 | if para.DebugPlot 151 | figure(1) 152 | showPoints(point) 153 | hold on 154 | showSuperellipse(x_n, 'Color', 'g') 155 | hold off 156 | title(['Trial', num2str(iterEM)]) 157 | pause 158 | end 159 | %------------------------------------------- 160 | 161 | if (cost_n < toleranceEM && iterEM > 1) || ... 162 | (relative_cost < relative_toleranceEM ... 163 | && switched >= para.MaxSwitch && iterEM > iterEM_min) 164 | x = x_n; 165 | %------------------------------------------- 166 | if para.DebugPlot 167 | figure(1) 168 | showPoints(point) 169 | hold on 170 | showSuperellipse(x, 'Color', 'g') 171 | hold off 172 | title('Final') 173 | pause 174 | end 175 | %------------------------------------------- 176 | break 177 | end 178 | 179 | % set different tolerance for switch and termination 180 | if relative_cost < relative_toleranceEM && iterEM ~= 1 181 | if para.DebugPlot 182 | disp('Enter switching...') 183 | end 184 | %------------------------------------------- 185 | if para.DebugPlot 186 | figure(1) 187 | showPoints(point) 188 | hold on 189 | showSuperellipse(x_n, 'Color', 'g') 190 | hold off 191 | title('Enter switch') 192 | pause 193 | end 194 | %------------------------------------------- 195 | % activate switching algorithm to avoid local minimum 196 | switch_success = 0; 197 | 198 | % duality similarity 199 | eul_rot = x(4) + 45 * pi / 180; 200 | eul_rot = atan(sin(eul_rot)/cos(eul_rot)); 201 | x_candidate = [max(2 - x(1), 1e-2), ((1 - sqrt(2)) * x(1) + sqrt(2)) * min(x(2), x(3)) * ones(1, 2), eul_rot, x(5 : 6)]; 202 | %------------------------------------------- 203 | if para.DebugPlot 204 | figure(1) 205 | showPoints(point) 206 | hold on 207 | showSuperellipse(x_n, 'Color', 'g') 208 | showSuperellipse(x_candidate, 'Color', 'b') 209 | hold off 210 | title('Candidate in blue') 211 | pause 212 | end 213 | %------------------------------------------- 214 | % introduce adaptive upper bound 215 | if adaptive_upper == 1 216 | R_current = angle2rotm(x_candidate(4)); 217 | point_rot_current = R_current' * point - R_current' * x_candidate(5 : 6)'; 218 | ub_a = 1.1 * [max(abs(point_rot_current(1, :))), max(abs(point_rot_current(2, :)))]; 219 | ub = [2.0 ub_a 2*pi ub_a]; 220 | lb = [0.0 0.01 0.01 -2*pi -ub_a]; 221 | end 222 | 223 | [x_switch, ~] = lsqnonlin(cost_func, x_candidate, lb, ub, options); 224 | %------------------------------------------- 225 | if para.DebugPlot 226 | figure(1) 227 | showPoints(point) 228 | hold on 229 | showSuperellipse(x_n, 'Color', 'g') 230 | showSuperellipse(x_switch, 'Color', 'b') 231 | hold off 232 | title('Candidate updated in blue') 233 | pause 234 | end 235 | %------------------------------------------- 236 | [dist_switch] = distance(point, x_switch); 237 | p = correspendence(dist_switch, sigma2, w, p0); 238 | cost_switch = sum(weighted_dist(x_switch, point, p).^2); 239 | if cost_switch < min(cost_n, cost) 240 | x = x_switch; 241 | cost = cost_switch; 242 | % update sigma 243 | sigma2 = cost_switch / (2 * sum(p)); 244 | switch_success = 1; 245 | %------------------------------------------- 246 | if para.DebugPlot 247 | figure(1) 248 | showPoints(point) 249 | hold on 250 | showSuperellipse(x_n, 'Color', 'g') 251 | showSuperellipse(x, 'Color', 'b') 252 | hold off 253 | title('Candidate switch success') 254 | disp('Swith succeed') 255 | pause 256 | end 257 | %------------------------------------------- 258 | end 259 | 260 | if switch_success == 0 261 | cost = cost_n; 262 | sigma2 = sigma2_n; 263 | x = x_n; 264 | %------------------------------------------- 265 | if para.DebugPlot 266 | figure(1) 267 | showPoints(point) 268 | hold on 269 | showSuperellipse(x_n, 'Color', 'g') 270 | hold off 271 | title('Candidate failed, return to x_n') 272 | disp('Fail in switch, return to x_n') 273 | pause 274 | end 275 | %------------------------------------------- 276 | end 277 | switched = switched + 1; 278 | else 279 | cost = cost_n; 280 | sigma2 = sigma2_n; 281 | x = x_n; 282 | end 283 | end 284 | % revert scaling 285 | if para.Rescale == 1 286 | x(2 : 3) = x(2 : 3) * scale; 287 | x(5 : 6) = x(5 : 6) * scale; 288 | end 289 | 290 | % transform back from the center of mass 291 | x(5 : 6) = x(5 : 6) + t0'; 292 | if para.DebugPlot 293 | disp('--------------------------------------------------') 294 | disp('Succeed!') 295 | end 296 | 297 | %% Functions 298 | % ------------------eigen analysis----------------------------------------- 299 | function [EigenVector, EigenValue] = EigenAnalysis(point) 300 | CovM = point * point' ./ size(point, 2); 301 | [EigenVector, EigenValue] = eig(CovM); 302 | EigenVector = flip(EigenVector, 2); 303 | end 304 | 305 | % ------------------distance function-------------------------------------- 306 | function [dist] = distance(X, para) 307 | % transform pose parameters into R matrix and t vector 308 | R = [cos(para(4)), -sin(para(4)); sin(para(4)), cos(para(4))]; 309 | t = para(5 : 6); 310 | % align the point cloud to the superquadrics coordinate 311 | X_c = R' * X - R' * t'; 312 | % calulate the radial distance of each point 313 | r_0 = vecnorm(X_c); 314 | dist = r_0 .* abs((((X_c(1, :) / para(2)) .^ (2)) .^ (1 / para(1)) + ... 315 | ((X_c(2, :) / para(3)) .^ (2)) .^ (1 / para(1))) .^ (-para(1) / 2) - 1); 316 | end 317 | 318 | % ------------------correspondence calculation ---------------------------- 319 | function [p] = correspendence(dist, sigma2, w, p0) 320 | c = (2 * pi * sigma2) ^ (- 1); 321 | const = (w * p0) / (c * (1 - w)); 322 | p = exp(-1 / (2 * sigma2) * dist .^ 2); 323 | p = p ./ (const + p); 324 | end 325 | 326 | % ------------------weighed distance function ----------------------------- 327 | function [value] = weighted_dist(para, X, p) 328 | value = p .^ (1 / 2) .* distance(X, para); 329 | 330 | end 331 | % ------------------ angle to rotm--------------------- 332 | function [value] = angle2rotm(a) 333 | value = [cos(a), -sin(a); sin(a), cos(a)]; 334 | end 335 | % ------------------parsing input arguments------------------------------- 336 | function [para] = parseInputArgs(point, varargin) 337 | 338 | [n_row, n_col] = size(point); 339 | % check for dimension of input 340 | if n_row ~= 2 341 | error('Input point cloud shoud be an array of 2 x N.') 342 | end 343 | % check for minumum points 344 | if n_col < 6 345 | error('Number of points less than 6.') 346 | end 347 | 348 | % set input parser 349 | defaults = struct('OutlierRatio', 0.8, ... 350 | 'MaxIterationEM', 30, ... 351 | 'ToleranceEM', 1e-5, ... 352 | 'RelativeToleranceEM', 1e-1, ... 353 | 'MaxOptiIterations', 10, ... 354 | 'Sigma', 0, ... 355 | 'MaxSwitch', 5, ... 356 | 'AdaptiveUpperBound', false, ... 357 | 'Rescale', false,... 358 | 'DebugPlot', false); 359 | 360 | parser = inputParser; 361 | parser.CaseSensitive = false; 362 | 363 | parser.addParameter('OutlierRatio', defaults.OutlierRatio, ... 364 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'nonnegative' , '>=', 0, '<', 1})); 365 | parser.addParameter('MaxIterationEM', defaults.MaxIterationEM, ... 366 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'integer', 'positive'})); 367 | parser.addParameter('ToleranceEM', defaults.ToleranceEM, ... 368 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'nonnegative'})); 369 | parser.addParameter('RelativeToleranceEM', defaults.RelativeToleranceEM, ... 370 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'nonnegative'})); 371 | parser.addParameter('MaxOptiIterations', defaults.MaxOptiIterations, ... 372 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'integer', 'positive'})); 373 | parser.addParameter('Sigma', defaults.Sigma, ... 374 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'nonnegative'})); 375 | parser.addParameter('MaxSwitch', defaults.MaxSwitch, ... 376 | @(x)validateattributes(x, {'single', 'double'}, {'real', 'nonsparse', 'nonempty', 'scalar', 'nonnan', 'finite', 'integer', 'nonnegative'})); 377 | parser.addParameter('AdaptiveUpperBound', defaults.AdaptiveUpperBound, @islogical); 378 | parser.addParameter('Rescale', defaults.Rescale, @islogical); 379 | parser.addParameter('DebugPlot', defaults.DebugPlot, @islogical); 380 | 381 | parser.parse(varargin{:}); 382 | para = parser.Results; 383 | end 384 | 385 | end 386 | --------------------------------------------------------------------------------