As-Conformal-As-Possible Surface Registration

最新推荐文章于 2022-11-09 09:58:42 发布

seamanj

最新推荐文章于 2022-11-09 09:58:42 发布

阅读量1k

点赞数

分类专栏： paper MATLAB

本文链接：https://blog.youkuaiyun.com/seamanj/article/details/51847040

版权

MATLAB 同时被 2 个专栏收录

108 篇文章

订阅专栏

paper

106 篇文章

订阅专栏

{3.2. Conformal stiffness energy

$LConformal(T) =\lVert T_{11} - T_{22}\rVert^2+\lVert T_{22} - T_{33}\rVert^2+\lVert T_{33} - T_{11}\rVert^2+\lVert T_{12} + T_{21}\rVert^2+\lVert T_{23} + T_{32}\rVert^2+\lVert T_{31} + T_{13}\rVert^2$

where

T = ⎡ ⎣ ⎢ T 11 T 21 T 31 T 12 T 22 T 32 T 13 T 23 T 33 ⎤ ⎦ ⎥

$T = \begin{bmatrix} T_{11} &T_{12}&T_{13}\\ T_{21} &T_{22}&T_{23}\\ T_{31} &T_{32}&T_{33}\\ \end{bmatrix}$

Minimizing this objective function is equivalent to the following equation set:
$\lVert T_{11} - T_{22}\rVert=0$
$\lVert T_{22} - T_{33}\rVert=0$
$\lVert T_{33} - T_{11}\rVert=0$
$\lVert T_{12} + T_{21}\rVert=0$
$\lVert T_{23} + T_{32}\rVert=0$
$\lVert T_{31} + T_{13}\rVert=0$
[see the fourth one in http://blog.youkuaiyun.com/seamanj/article/details/51803822 , which can be applied in following derivations]

which can be written in matrix form:

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 10 - 1 000 000100000001000100 - 1 10000 000010000001000010 0 - 1 1000 000000000000000000 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ T 11 T 21 T 31 T 12 T 22 T 32 T 13 T 23 T 33 t 1 t 2 t 3 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 000000 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$\begin{bmatrix} 1 &0&0&0&-1&0&0&0&0&0&0&0\\ 0 &0&0&0&1&0&0&0&-1&0&0&0\\ -1 &0&0&0&0&0&0&0&1&0&0&0\\ 0 &1&0&1&0&0&0&0&0&0&0&0\\ 0 &0&0&0&0&1&0&1&0&0&0&0\\ 0 &0&1&0&0&0&1&0&0&0&0&0\\ \end{bmatrix}\begin{bmatrix} T_{11} \\ T_{21} \\ T_{31} \\ T_{12} \\ T_{22} \\ T_{32} \\ T_{13} \\ T_{23} \\ T_{33} \\ t_{1} \\ t_{2} \\ t_{3} \\ \end{bmatrix}=\begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix}$

}

{consist energe
$\forall i, \forall j\in N(i)$ , $\lVert T_i(v_j^0-v_i^0)+v_i^0+t_i-(v_j^0+t_j) \rVert=0$
writing in matrix form leads to:

⎡ ⎣ ⎢ T i (11) T i (21) T i (31) T i (12) T i (22) T i (32) T i (13) T i (23) T i (33) ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ ⎢ v 0 j (1) - v 0 i (1) v 0 j (2) - v 0 i (2) v 0 j (3) - v 0 i (3) ⎤ ⎦ ⎥ ⎥ + ⎡ ⎣ ⎢ ⎢ v 0 i (1) v 0 i (2) v 0 i (3) ⎤ ⎦ ⎥ ⎥ + ⎡ ⎣ ⎢ t i (1) t i (2) t i (3) ⎤ ⎦ ⎥ - ⎡ ⎣ ⎢ ⎢ v 0 j (1) + t j (1) v 0 j (2) + t j (2) v 0 j (3) + t j (3) ⎤ ⎦ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ T i (11) (v 0 j (1) - v 0 i (1)) + T i (12) (v 0 j (2) - v 0 i (2)) + T i (13) (v 0 j (3) - v 0 i (3)) + v 0 i (1) + t i (1) - v 0 j (1) - t j (1) T i (21) (v 0 j (1) - v 0 i (1)) + T i (22) (v 0 j (2) - v 0 i (2)) + T i (23) (v 0 j (3) - v 0 i (3)) + v 0 i (1) + t i (2) - v 0 j (2) - t j (2) T i (31) (v 0 j (1) - v 0 i (1)) + T i (32) (v 0 j (2) - v 0 i (2)) + T i (33) (v 0 j (3) - v 0 i (3)) + v 0 i (1) + t i (3) - v 0 j (3) - t j (3) ⎤ ⎦ ⎥ ⎥ = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥

$\begin{bmatrix} T_i(11) &T_i(12)&T_i(13)\\ T_i(21) &T_i(22)&T_i(23)\\ T_i(31) &T_i(32)&T_i(33)\\ \end{bmatrix}\begin{bmatrix} v_j^0(1)-v_i^0(1)\\ v_j^0(2)-v_i^0(2)\\ v_j^0(3)-v_i^0(3)\\ \end{bmatrix}+\begin{bmatrix} v_i^0(1)\\ v_i^0(2)\\ v_i^0(3)\\ \end{bmatrix}+\begin{bmatrix} t_i(1)\\ t_i(2)\\ t_i(3)\\ \end{bmatrix}-\begin{bmatrix} v_j^0(1)+t_j(1)\\ v_j^0(2)+t_j(2)\\ v_j^0(3)+t_j(3)\\ \end{bmatrix}\\ =\begin{bmatrix} T_i(11)(v_j^0(1)-v_i^0(1)) +T_i(12)(v_j^0(2)-v_i^0(2))+T_i(13)(v_j^0(3)-v_i^0(3))+v_i^0(1)+t_i(1)-v_j^0(1)-t_j(1)\\ T_i(21)(v_j^0(1)-v_i^0(1)) +T_i(22)(v_j^0(2)-v_i^0(2))+T_i(23)(v_j^0(3)-v_i^0(3))+v_i^0(1)+t_i(2)-v_j^0(2)-t_j(2)\\ T_i(31)(v_j^0(1)-v_i^0(1)) +T_i(32)(v_j^0(2)-v_i^0(2))+T_i(33)(v_j^0(3)-v_i^0(3))+v_i^0(1)+t_i(3)-v_j^0(3)-t_j(3)\\ \end{bmatrix}=\begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}$

Collecting all the unknown variables into a column vector yields us:

⎡ ⎣ ⎢ ⎢ v 0 j (1) - v 0 i (1) 00 0 v 0 j (1) - v 0 i (1) 0 00 v 0 j (1) - v 0 i (1) v 0 j (2) - v 0 i (2) 00 0 v 0 j (2) - v 0 i (2) 0 00 v 0 j (2) - v 0 i (2) v 0 j (3) - v 0 i (3) 00 0 v 0 j (3) - v 0 i (3) 0 00 v 0 j (3) - v 0 i (3) 100010001 ⎤ ⎦ ⎥ ⎥ \dots ⎡ ⎣ ⎢ - 1 00 0 - 1 0 00 - 1 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ T i (11) T i (21) T i (31) T i (12) T i (22) T i (32) T i (13) T i (23) T i (33) t i (1) t i (2) t i (3) ⋮ t j (1) t j (2) t j (3) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ v 0 j (1) - v 0 i (1) v 0 j (2) - v 0 i (2) v 0 j (3) - v 0 i (3) ⎤ ⎦ ⎥ ⎥

$\begin{bmatrix} v_j^0(1)-v_i^0(1) &0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)&0&0&1&0&0\\ 0 &v_j^0(1)-v_i^0(1)&0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)&0&0&1&0\\ 0 &0&v_j^0(1)-v_i^0(1)&0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)&0&0&1\\ \end{bmatrix}\cdots\begin{bmatrix} -1 &0&0\\ 0 &-1&0\\ 0 &0&-1\\ \end{bmatrix} \begin{bmatrix} T_i(11) \\ T_i(21) \\ T_i(31) \\ T_i(12) \\ T_i(22) \\ T_i(32) \\ T_i(13) \\ T_i(23) \\ T_i(33) \\ t_i(1) \\ t_i(2) \\ t_i(3) \\ \vdots\\ t_j(1) \\ t_j(2) \\ t_j(3) \end{bmatrix}=\begin{bmatrix} v_j^0(1)-v_i^0(1)\\ v_j^0(2)-v_i^0(2)\\ v_j^0(3)-v_i^0(3)\\ \end{bmatrix}$
这里写图片描述

}

{smooth energe
$\forall i, \forall j\in N(i)$ , $\lVert T_i(v_j^0-v_i^0)+T_j(v_i^0-v_j^0) \rVert=0$
writing in matrix form leads to:

⎡ ⎣ ⎢ T i (11) T i (21) T i (31) T i (12) T i (22) T i (32) T i (13) T i (23) T i (33) ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ ⎢ v 0 j (1) - v 0 i (1) v 0 j (2) - v 0 i (2) v 0 j (3) - v 0 i (3) ⎤ ⎦ ⎥ ⎥ + ⎡ ⎣ ⎢ T j (11) T j (21) T j (31) T j (12) T j (22) T j (32) T j (13) T j (23) T j (33) ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ ⎢ v 0 i (1) - v 0 j (1) v 0 i (2) - v 0 j (2) v 0 i (3) - v 0 j (3) ⎤ ⎦ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ T i (11) (v 0 j (1) - v 0 i (1)) + T i (12) (v 0 j (2) - v 0 i (2)) + T i (13) (v 0 j (3) - v 0 i (3)) + T j (11) (v 0 i (1) - v 0 j (1)) + T j (12) (v 0 i (2) - v 0 j (2)) + T j (13) (v 0 i (3) - v 0 j (3)) T i (21) (v 0 j (1) - v 0 i (1)) + T i (22) (v 0 j (2) - v 0 i (2)) + T i (23) (v 0 j (3) - v 0 i (3)) + T j (21) (v 0 i (1) - v 0 j (1)) + T j (22) (v 0 i (2) - v 0 j (2)) + T j (23) (v 0 i (3) - v 0 j (3)) T i (31) (v 0 j (1) - v 0 i (1)) + T i (32) (v 0 j (2) - v 0 i (2)) + T i (33) (v 0 j (3) - v 0 i (3)) + T j (31) (v 0 i (1) - v 0 j (1)) + T j (32) (v 0 i (2) - v 0 j (2)) + T j (33) (v 0 i (3) - v 0 j (3)) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥

$\begin{bmatrix} T_i(11) &T_i(12)&T_i(13)\\ T_i(21) &T_i(22)&T_i(23)\\ T_i(31) &T_i(32)&T_i(33)\\ \end{bmatrix}\begin{bmatrix} v_j^0(1)-v_i^0(1)\\ v_j^0(2)-v_i^0(2)\\ v_j^0(3)-v_i^0(3)\\ \end{bmatrix}+\begin{bmatrix} T_j(11) &T_j(12)&T_j(13)\\ T_j(21) &T_j(22)&T_j(23)\\ T_j(31) &T_j(32)&T_j(33)\\ \end{bmatrix}\begin{bmatrix} v_i^0(1)-v_j^0(1)\\ v_i^0(2)-v_j^0(2)\\ v_i^0(3)-v_j^0(3)\\ \end{bmatrix}\\ =\begin{bmatrix} T_i(11)(v_j^0(1)-v_i^0(1)) +T_i(12)(v_j^0(2)-v_i^0(2))+T_i(13)(v_j^0(3)-v_i^0(3))+T_j(11)(v_i^0(1)-v_j^0(1)) +T_j(12)(v_i^0(2)-v_j^0(2))+T_j(13)(v_i^0(3)-v_j^0(3))\\ T_i(21)(v_j^0(1)-v_i^0(1)) +T_i(22)(v_j^0(2)-v_i^0(2))+T_i(23)(v_j^0(3)-v_i^0(3))+T_j(21)(v_i^0(1)-v_j^0(1)) +T_j(22)(v_i^0(2)-v_j^0(2))+T_j(23)(v_i^0(3)-v_j^0(3))\\ T_i(31)(v_j^0(1)-v_i^0(1)) +T_i(32)(v_j^0(2)-v_i^0(2))+T_i(33)(v_j^0(3)-v_i^0(3))+T_j(31)(v_i^0(1)-v_j^0(1)) +T_j(32)(v_i^0(2)-v_j^0(2))+T_j(33)(v_i^0(3)-v_j^0(3))\\ \end{bmatrix}=\begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}$

⎡ ⎣ ⎢ ⎢ v 0 j (1) - v 0 i (1) 00 0 v 0 j (1) - v 0 i (1) 0 00 v 0 j (1) - v 0 i (1) v 0 j (2) - v 0 i (2) 00 0 v 0 j (2) - v 0 i (2) 0 00 v 0 j (2) - v 0 i (2) v 0 j (3) - v 0 i (3) 00 0 v 0 j (3) - v 0 i (3) 0 00 v 0 j (3) - v 0 i (3) ⎤ ⎦ ⎥ ⎥ \dots ⎡ ⎣ ⎢ ⎢ v 0 i (1) - v 0 j (1) 00 0 v 0 i (1) - v 0 j (1) 0 00 v 0 i (1) - v 0 j (1) v 0 i (2) - v 0 j (2) 00 0 v 0 i (2) - v 0 j (2) 0 00 v 0 i (2) - v 0 j (2) v 0 i (3) - v 0 j (3) 00 0 v 0 i (3) - v 0 j (3) 0 00 v 0 i (3) - v 0 j (3) ⎤ ⎦ ⎥ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ T i (11) T i (21) T i (31) T i (12) T i (22) T i (32) T i (13) T i (23) T i (33) ⋮ T j (11) T j (21) T j (31) T j (12) T j (22) T j (32) T j (13) T j (23) T j (33) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥

$\begin{bmatrix} v_j^0(1)-v_i^0(1) &0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)&0&0\\ 0 &v_j^0(1)-v_i^0(1)&0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)&0\\ 0 &0&v_j^0(1)-v_i^0(1)&0&0&v_j^0(2)-v_i^0(2)&0&0&v_j^0(3)-v_i^0(3)\\ \end{bmatrix}\cdots\begin{bmatrix} v_i^0(1)-v_j^0(1) &0&0&v_i^0(2)-v_j^0(2)&0&0&v_i^0(3)-v_j^0(3)&0&0\\ 0 &v_i^0(1)-v_j^0(1)&0&0&v_i^0(2)-v_j^0(2)&0&0&v_i^0(3)-v_j^0(3)&0\\ 0 &0&v_i^0(1)-v_j^0(1)&0&0&v_i^0(2)-v_j^0(2)&0&0&v_i^0(3)-v_j^0(3)\\ \end{bmatrix} \begin{bmatrix} T_i(11) \\ T_i(21) \\ T_i(31) \\ T_i(12) \\ T_i(22) \\ T_i(32) \\ T_i(13) \\ T_i(23) \\ T_i(33) \\ \vdots\\ T_j(11) \\ T_j(21) \\ T_j(31) \\ T_j(12) \\ T_j(22) \\ T_j(32) \\ T_j(13) \\ T_j(23) \\ T_j(33) \end{bmatrix}=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix}$

这里写图片描述

}

At last, I would like release the matlab source of this paper:

clear;
consist = 1;
smooth = 1;
conformal = 1;
feature = 1;
closest = 1;


X0=read_obj('sphere_before.obj');
V=X0.xyz';  % list of vertex positions
F=X0.tri'; % list of triangles indices
N=X0.vertex_mean_normal';
n = size(V,1); %number of vertices


if( feature || closest )
     X1=read_obj('sphere_ffd.obj'); 
     V_target=X1.xyz';  % list of vertex positions
     F_target=X1.tri';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%finding neighbour%%%%%%%%%%%%%%%%%%%%%%%%%%%%
neighbour_matrix = zeros(n,n);
for i=1:n
    neighbour = find(F(:,1)==i | F(:,2)==i | F(:,3)==i);
    neighbour = F(neighbour,:);
    neighbour = setdiff(neighbour(:),i);
    neighbour_matrix(i,neighbour(:)) = ones(1,size(neighbour,2));
end
 neighbour_count = sum(sum(neighbour_matrix,2),1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%consist%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(consist)
E_consist_A=zeros(3*neighbour_count, 12*n);
E_consist_b=zeros(3*neighbour_count, 1);
tic
%%construct matrix for consist energe
count = 1;
for i = 1:n
    for j = 1:n
        if( neighbour_matrix(i,j) && i~=j)
             E_consist_A((count-1)*3+1 : count*3, (i-1)*12 + 1 : i*12) = ...
                 [eye(3)*(V(j,1)-V(i,1)) eye(3)*(V(j,2)-V(i,2)) eye(3)*(V(j,3)-V(i,3)) eye(3)];
             E_consist_A((count-1)*3+1 : count*3, (j-1)*12 + 10 : j*12) = ...
                  -1 * eye(3);
             E_consist_b((count-1)*3+1 : count*3,1) = V(j,:)' - V(i,:)';
             count = count + 1;
        end
    end
end
t = toc;
fprintf('construction for consist energe completes: %gs\n',t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%smooth%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(smooth)
E_smooth_A=zeros(3*neighbour_count, 12*n);
E_smooth_b=zeros(3*neighbour_count, 1);    
tic
%construct matrix for smooth energe
count = 1;
for i = 1:n
    for j = 1:n
        if( neighbour_matrix(i,j) && i~=j)
            % E_smooth_A_temp = sparse(3, 12*n);
            E_smooth_A( (count-1)*3+1 : count*3, (i-1)*12 + 1 : (i-1)*12 + 9) = ...
            [eye(3)*(V(j,1)-V(i,1)) eye(3)*(V(j,2)-V(i,2)) eye(3)*(V(j,3)-V(i,3))];
            E_smooth_A( (count-1)*3+1 : count*3, (j-1)*12 + 1 : (j-1)*12 + 9) = ...
            [eye(3)*(V(i,1)-V(j,1)) eye(3)*(V(i,2)-V(j,2)) eye(3)*(V(i,3)-V(j,3))];
            count = count + 1;
        end
    end
end
t = toc;
fprintf('construction for smooth energe completes: %gs\n',t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%conformal%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(conformal)
tic
%construct matrix for linear conformal energe
E_lconformal_A = zeros(6*n, 12*n);
E_lconformal_b = zeros(6*n, 1);
for i = 1:n

             E_lconformal_A( (i-1)*6 + 1 : i*6, (i-1)*12 + 1 : (i-1)*12 + 9) = ...
             [ 1 0 0   0 -1 0   0 0  0;
               0 0 0   0  1 0   0 0 -1;
              -1 0 0   0  0 0   0 0  1;
               0 1 0   1  0 0   0 0  0;
               0 0 0   0  0 1   0 1  0;
               0 0 1   0  0 0   1 0  0];    

end
t = toc;
fprintf('construction for linear conformal energe completes: %gs\n',t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%feature%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(feature)
tic
%construct matrix for feature point constraints


feature_matching_pairs = [1:200; 1:200]';

E_feature_A = zeros(3*n, 12*n);
E_feature_b = zeros(3*n, 1);

for i = 1:size(feature_matching_pairs,1)
    template_index = feature_matching_pairs(i,1);
    target_index = feature_matching_pairs(i,2);

    E_feature_A( (template_index-1)*3+1 : template_index*3, (template_index-1)*12+10 : template_index*12) = ...
    eye(3);
    E_feature_b((template_index-1)*3+1 : template_index*3) = V_target(target_index,:) - V(template_index,:);
end
t = toc;
fprintf('construction for feature point constraints completes: %gs\n',t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%closest%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(closest)
tic
%construct matrix for closest point constraints
closest_matching_pairs = [1:200; 1:200]';
E_closest_A = zeros(3*n, 12*n);
E_closest_b = zeros(3*n, 1);
for i = 1:size(closest_matching_pairs,1)
    template_index = closest_matching_pairs(i,1);
    target_index = closest_matching_pairs(i,2);

    E_closest_A( (template_index-1)*3+1 : template_index*3, (template_index-1)*12+10 : template_index*12) = ...
    eye(3);
    VP = V_target(target_index,:) - V(template_index,:);
    Normal = N(template_index,:);
    E_closest_b((template_index-1)*3+1 : template_index*3) = dot(VP,Normal).*Normal;
end
t = toc;
fprintf('construction for cloest point constraints completes: %gs\n',t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%total%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
%construct the total system
A=[];
b=[];
if(consist)
    A = [A; E_consist_A];
    b = [b; E_consist_b];
end
if(smooth)
    A = [A; E_smooth_A];
    b = [b; E_smooth_b];
end
if(conformal)
    A = [A; E_lconformal_A];
    b = [b; E_lconformal_b];    
end
if(feature)
    A = [A; E_feature_A];
    b = [b; E_feature_b];    
end
if(closest)
    A = [A; E_closest_A];
    b = [b; E_closest_b];    
end
t = toc;
fprintf('construction for total system completes: %gs\n',t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%solving%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
%solve the whole system
%T=linsolve(A'*A,A'*b);%7.13842s

T = (A'*A)\(A'*b);%6.63639s

%T=pcg(A'*A,A'*b,1e-12,2000);%9.59388s 
%pcg converged at iteration 649 to a solution with relative residual 8.8e-13.

%  A = sparse(A);
%  b = sparse(b);
%  [L,U,P,Q,R] = lu(A);
%  T = Q*(U\(L\(P*(R\b))));%0.724697s


t = toc;
fprintf('solving completes: %gs\n',t);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%update%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:n
    V(i,:) = V(i,:) + T((i-1)*12+10 : i*12)';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
hold on
trimesh(F,V(:,1),V(:,2),V(:,3),'EdgeColor','b'); 
%trimesh(F_target,V_target(:,1),V_target(:,2),V_target(:,3),'EdgeColor','r'); 
axis equal
view(3)
disp('done!')

>> acap
construction for consist energe completes: 0.0397842s
construction for smooth energe completes: 0.0420681s
construction for linear conformal energe completes: 0.0217301s
construction for feature point constraints completes: 0.0140695s
construction for cloest point constraints completes: 0.0417374s
construction for total system completes: 1.08451s
solving completes: 4.71639s
done!
>>