Add optimization functions

matlab/demo.m

@@ -52,12 +52,19 @@
 % linear solution with proposed method
 [R_t, T_t, n_t, d_t, rep_t] = tnm(model, input, in_param);
 
-% result
+% non linear refinement (minimizing reprojection error)
+[R_t_opt, T_t_opt, n_t_opt, d_t_opt, rep_t_opt] = ...
+    opt(model, input, in_param, R_t, T_t, n_t, d_t);
 
+% result
 fprintf('\n');
 fprintf('Average reprojection error by TNM : %f pixel.\n', rep_t);
 fprintf('\n');
 
+fprintf('\n');
+fprintf('Average reprojection error by TNM (with non-linear refinement): %f pixel.\n', rep_t_opt);
+fprintf('\n');
+
 fprintf('==== Parameters by TNM ====\n');
 R = R_t
 T = T_t
@@ -68,6 +75,16 @@
 d2 = d_t(2,:)
 d3 = d_t(3,:)
 
+fprintf('==== Parameters by TNM (with non-linear refinement)====\n');
+R_opt = R_t_opt
+T_opt = T_t_opt
+n1_opt = n_t_opt(1,:)'
+n2_opt = n_t_opt(2,:)'
+n3_opt = n_t_opt(3,:)'
+d1_opt = d_t_opt(1,:)
+d2_opt = d_t_opt(2,:)
+d3_opt = d_t_opt(3,:)
+
 % display the result
 
 az = 10;
@@ -98,6 +115,16 @@
 text(Cp_t(1,1) + offset, Cp_t(1,2) + offset, Cp_t(1,3) + offset, ...
     'Reference Points Estimated by TNM');
 
+% plot parameters by TNM (with non-linear refinement)
+sub_plot_axis(R_t_opt, T_t_opt);
+
+% plot reference points estimated by TNM
+Cp_t_opt = (R_t_opt * model' + repmat(T_t_opt, 1, size(model,1)))';
+sub_plot_plane(Cp_t_opt, 'm', 3, 1);
+text(Cp_t_opt(1,1) + offset, Cp_t_opt(1,2) + offset, Cp_t_opt(1,3) + offset, ...
+    'Reference Points Estimated by TNM (with non-linear refinement)');
+
+
 % plot mirrored reference points
 Cp_t_h = Cp_t;
 Cp_t_h(:,4) = 1;

matlab/opt.m

@@ -0,0 +1,46 @@
+function [R_opt, T_opt, n_opt, d_opt, rep_opt] = ...
+    opt(Xp, q, in_param, R, T, n, d)
+% OPT
+% OUTPUT 
+%       R_opt: optimized Rotation matrix.
+%       T_opt: optimized Translation vector.
+%       n_opt: optimized mirror normal vector.
+%       d_opt: optimized distance betweeen camera and mirror.
+%       rep_opt: optimized reprojection error when using estimated parameters
+%
+% INPUT
+%       Xp: 3D coordinate of reference points in the reference coordinate system
+%       q: 2D coordinate of mirrored reference points on the image plane.
+%       in_param: intrinsic camera parameters
+% 
+
+num_of_mirror_pose = size(q, 1);
+
+[R_x R_y R_z] = sub_angle_from_matrix(R);
+init_value = [R_x; R_y; R_z; T];
+
+for i = 1:num_of_mirror_pose
+  [n_theta n_phi] = sub_angle_from_vector(n(i,:)');
+  init_value = [init_value; n_theta; n_phi];
+end
+
+init_value = [init_value; d];
+opt = optimset('Display', 'off');
+opt = optimset(opt, 'Algorithm', 'levenberg-marquardt');
+
+[x, fval, exitflag, output] = ...
+    fsolve(@opt_func, init_value, opt, Xp, q, in_param);
+
+R_opt = sub_matrix_from_angle(x(1,1),x(2,1),x(3,1));
+T_opt = x(4:6,1);
+
+sum_error_opt = 0;
+
+for i = 1:num_of_mirror_pose
+  n_opt_theta = x(2*(i-1)+6+1,1);
+  n_opt_phi = x(2*(i-1)+6+2,1);
+  n_opt(i,:) = (sub_vector_from_angle(n_opt_theta, n_opt_phi))';
+  d_opt(i,1) = x(i+6+2*num_of_mirror_pose,1);
+end
+
+rep_opt = sub_reproj(Xp, q, R_opt, T_opt, n_opt, d_opt, in_param);

matlab/opt_func.m

@@ -0,0 +1,38 @@
+function F = opt_func(x, Xp, q, in_param)
+
+% OPT FUNC
+% OUTPUT 
+%       F: objective function
+%
+% INPUT
+%       x: parameters to optimize
+%       Xp: 3D coordinate of reference points in the reference coordinate system
+%       q: 2D coordinate of mirrored reference points on the image plane.
+%       in_param: intrinsic camera parameters
+% 
+
+num_of_mirror_pose = size(q,1);
+
+R_x = x(1,1);
+R_y = x(2,1);
+R_z = x(3,1);
+R = sub_matrix_from_angle(R_x, R_y, R_z);
+T = x(4:6,1);
+
+for i = 1:num_of_mirror_pose
+  n_theta = x(2*(i-1)+6+1,1);
+  n_phi = x(2*(i-1)+6+2,1);
+  n = sub_vector_from_angle(n_theta, n_phi);
+
+  d = x(i+6+2*num_of_mirror_pose,1);
+
+  temp_q = q{i,1};
+
+  reps = (sub_reproj_core(Xp, temp_q, R, T, n, d, in_param))';
+
+  if i == 1
+    F = reps(:);
+  else
+    F = [F; reps(:)];
+  end
+end

matlab/sub_angle_from_matrix.m

@@ -0,0 +1,34 @@
+function [x_angle y_angle z_angle] = sub_angle_from_matrix(R)
+
+% SUB ANGLE FROM MATRIX
+% OUTPUT 
+%       x_angle, y_angle, z_angle: rotation angle for each axis
+%
+% INPUT
+%       R: Rotation matrix
+% 
+
+threshold = 1.0e-10;
+
+% in the case of R(3,1) = -sin(y) = -1
+% sin(y) = 1 
+if abs(R(3,1) + 1.0) < threshold
+
+  x_angle = 0;
+  y_angle = pi / 2;
+  z_angle = atan2(-1 * R(1,2), R(2,2));
+
+elseif abs(R(3,1) - 1.0) < threshold % R(3,1) = -sin(y) = 1
+
+  x_angle = 0;
+  y_angle = -pi / 2;
+  z_angle = atan2(-1 * R(1,2), R(2,2));
+
+else
+
+  x_angle = atan2(R(3,2), R(3,3));
+  y_angle = asin(-1 * R(3,1));
+  z_angle = atan2(R(2,1), R(1,1));
+
+end
+

matlab/sub_angle_from_vector.m

@@ -0,0 +1,15 @@
+function [theta phi] = computeAngleFromVector(vec)
+
+% SUB ANGLE FROM VECTOR
+% OUTPUT 
+%       theta, phi: angles in the spherical polar coordinate system (r = 1)
+%
+% INPUT
+%       vec: vector
+% 
+
+theta = asin(vec(3,1));
+
+phi1 = asin(vec(2,1) / cos(theta));
+phi2 = acos(vec(1,1) / cos(theta));
+phi = (phi1 + phi2) / 2;

matlab/sub_matrix_from_angle.m

@@ -0,0 +1,19 @@
+function R = sub_matrix_from_angle(x_angle, y_angle, z_angle)
+
+% SUB MATRIX FROM ANGLE
+% OUTPUT 
+%       R: Rotation matrix
+%
+% INPUT
+%       x_angle, y_angle, z_angle: rotation angle for each axis
+% 
+
+R = [cos(y_angle)*cos(z_angle), ...
+     sin(x_angle)*sin(y_angle)*cos(z_angle) - cos(x_angle)*sin(z_angle), ...
+     cos(x_angle)*sin(y_angle)*cos(z_angle) + sin(x_angle)*sin(z_angle); ...
+     cos(y_angle)*sin(z_angle),...
+     sin(x_angle)*sin(y_angle)*sin(z_angle) + cos(x_angle)*cos(z_angle), ...
+     cos(x_angle)*sin(y_angle)*sin(z_angle) - sin(x_angle)*cos(z_angle);...
+     -sin(y_angle),...
+     sin(x_angle)*cos(y_angle),...
+     cos(x_angle)*cos(y_angle)];

matlab/sub_plot_plane.m

@@ -34,7 +34,7 @@
 hold on;
 
 num_of_points = size(points, 1);
-points(num_of_points+1,:) = points(1,:)
+points(num_of_points+1,:) = points(1,:);
 
 pLocusX = points(:,1);
 pLocusY = points(:,2);

matlab/sub_vector_from_angle.m

@@ -0,0 +1,13 @@
+function vec = sub_vector_from_angle(theta, phi)
+
+% SUB VECTOR FROM ANGLE
+% OUTPUT 
+%       vec: vector
+%
+% INPUT
+%       theta, phi: angles in the sphirical polar coordinate system (r = 1)
+% 
+
+vec(1,1) = cos(theta)*cos(phi);
+vec(2,1) = cos(theta)*sin(phi);
+vec(3,1) = sin(theta);