Algorithm 2.

ISMDM algorithm

for k = 0 to S − 1 do

for j = 0 to J − 1 do

uij ← arg maxu∈{ui} ||u − vj||

end for

õ ← arg minv∈{vj}(minu∈{ui} ||u − v||)

for j = 0 to J − 1 do

if ||uij − vj|| < ρ

Add the linear constraint ${[{\mathbf{v}}_j^{\prime }(\stackrel{\sim }{\mathbf{R}},\stackrel{\sim }{\mathbf{t}})-{\mathbf{r}}_j]}^T{\mathbf{n}}_j>0$

end if

end for

Minimize (14) by quadratic programming

Update vj using (7)

end for