Loop Closure Calculation (And Yet Another Application of Quaternion)

An inherent problem of registration within multiple captures is error accumulation. Assuming that we take n 3D captures, we can find 3D transfer between all consecutive captures. That should in turn enable us to calculate 3D transfer between any two of these captures such as between view 1 and view n. However, 3D transfer between view 1 and view n can be quite noisy. The reason is that 3D transfer of each pair of consecutive captures has a small error, and combining them together to get 3D transfer between view 1 and n add those errors together. This is called error accumulation.

One effective way to combat error accumulation is called loop closure. When you take multiple captures, a loop may be formed. For instance, when you use a camera to take a panorama view, view 1 and view n overlap. Then there should exist two ways to derive the relative location difference between view 1 and n. The first way is to combine 1->2, 2->3, ..., n-1->n. The second way is to directly find out from n->1 since view 1 and n ends at roughly the same place and form a loop. Due to error accumulation, result obtained by the first way often turns out to be less accurate than the result got from the second method. Thus we can use the second result to calibrate the first result. The outcome of this loop closure technique is that each estimate of 1->2 to n-1->n becomes more accurate.

A seminal paper about loop closure is "Multiview Registration of 3D Scenes by Minimizing Error between Coordinate Frames" written by G C Sharp et. al. Our coding mainly follows this paper and we will call this paper "the paper" for later text. Assuming from view i to view j, the 3D transformation matrix is R(i,j) and offset is t(i.j) where R is a 3-by-3 matrix and t is a 3-by-1 vector, we know R(1,2), R(2,3), ..., R(n-1,n), R(n,1) as well as t(1,2), t(2,3), ..., t(n-1,n), t(n,1). Based on Eq. (11) in the paper, we have:
R(1,2)...R(k,k+1)E(k,k+1)R(k+1,k+2)...R(n,1)=I
where I is a 3-by-3 identity matrix and E(k,k+1) is a 3-by-3 matrix to be found out. We will present Matlab code to calculate E(k,k+1). In Matlab code, R is a matrix which has 3*(n+1) lines and 4 rows. R has format as:
[R(1,2) t(1,2)]
[R(2,3) t(2,3)]
...
[R(n-1,n) t(n-1,n)]
[R(n,1) t(n,1)]
The code to calculate E(k,k+1) is:

Ralpha = zeros(mx, 3); Rbeta = zeros(mx, 3); 
for i = 1:mx/3
    if (i == 1)
        Ralpha(3*(i-1)+1:3*(i-1)+3,1:3) = R(3*(i-1)+1:3*(i-1)+3,1:3);
    else
        Ralpha(3*(i-1)+1:3*(i-1)+3,1:3) = Ralpha(3*(i-2)+1:3*(i-2)+3,1:3)*R(3*(i-1)+1:3*(i-1)+3,1:3);
    end
end

for i=mx/3:-1:1
    if (i == mx/3)
        Rbeta(3*(i-1)+1:3*(i-1)+3,1:3) = R(3*(i-1)+1:3*(i-1)+3,1:3);
    else
        Rbeta(3*(i-1)+1:3*(i-1)+3,1:3) = R(3*(i-1)+1:3*(i-1)+3,1:3)*Rbeta(3*(i)+1:3*(i)+3,1:3);
    end
end 

Emat = zeros(mx, 3);Emat_divN = zeros(mx, 3);
for i = 1:mx/3
    if (i ~= mx/3)
        Emat(3*(i-1)+1:3*(i-1)+3,1:3) = Ralpha(3*(i-1)+1:3*(i-1)+3,1:3)'*Rbeta(3*i+1:3*i+3,1:3)';
    else
        Emat(3*(i-1)+1:3*(i-1)+3,1:3) = Ralpha(3*(i-1)+1:3*(i-1)+3,1:3)';
    end
end

After calculating E(k,k+1), according to Eq. (22) of the paper we need to find the 1/n power of E(k,k+1). Without quaternion, it will be clueless to find the 1/n power of a rotation matrix. But by using quaternion, this become straightforward. We first convert E(k,k+1) to quaternion, divide the rotation angle by n, and then convert it back with the following Matlab code:

for i = 1:mx/3
    [w, x, y, z] = R2quaternion(Emat(3*(i-1)+1:3*(i-1)+3,1:3));
    w_new=cos(acos(w)/(mx/3));
    x_new=x*sin(acos(w)/(mx/3))/sin(acos(w));
    y_new=y*sin(acos(w)/(mx/3))/sin(acos(w));
    z_new=z*sin(acos(w)/(mx/3))/sin(acos(w));
    Emat_divN(3*(i-1)+1:3*(i-1)+3,1:3) = quaternion2R(w_new,x_new,y_new,z_new);
end

The final step is to use E_divN(k,k+1) to calibrate R(k,k+1):

Rtilda = zeros(mx, 4);
for i = 1:mx/3
    Rtilda(3*(i-1)+1:3*(i-1)+3,1:3) = R(3*(i-1)+1:3*(i-1)+3,1:3)*Emat_divN(3*(i-1)+1:3*(i-1)+3,1:3); 
end

For vector t, we use the simplified approach to calculate the average of all t vectors and deduct this average from each t:

tsum = zeros(3,1);
for i = 1:mx/3
    tsum = tsum+R(3*(i-1)+1:3*(i-1)+3,4);
end
tavg = tsum/(mx/3);

followed by

for i = 1:mx/3
    Rtilda(3*(i-1)+1:3*(i-1)+3,4) = R(3*(i-1)+1:3*(i-1)+3,4)-tavg; 
end

The whole Matlab code can be downloaded from here: https://drive.google.com/folderview?id=0B05MVyfGr8lmQWZVV25qQWpmbkE&usp=sharing