Using 3D RANSAC to estimate the 3D affine transform - matlab

Question

I am trying to register two volumeetric images(img1 and img2). The size of the img1 is 40x40x24. The size of the img2 is 64 x64x11.
So far, I have extracted their features (vol1 and vol2, the same size as the images) and then matched them.
Now, I have a set of corresponding points in two feature volumes that is stored in pairs which is a matrix of size 100x6 (every row of pairs is [x, y, z, X, Y, Z] where (x,y,z) are the coordinates of a voxel in vol1 and [X Y Z] is the coordinates of corresponding voxel in vol2).
Now, I am trying to use RANSAC algorithm to estimate the 3D affine transform, T. I have written the below code, but I think there is a problem with it because when I get the output transform T and multiply it by a sample voxel coordinates from vol1, I get some negative coordinates!!!
Below is my implementation for 3D RANSAC algorithm. I have used 2D RANSAC implementation in this link. Please let me know if you see any problem.
function [bp] = ransac(data,bpI,iter,num,distThresh)
% data: a nx6 dataset with #n data points
% num: the minimum number of points. Here num=4.
% iter: the number of iterations
% distThresh: the threshold of the distances between points and the fitting line
% inlierRatio: the threshold of the number of inliers
% bpI : Initialized affine transform model
number = size(data,1); % Total number of points
bestInNum = 0; % Best fitting line with largest number of inliers
% Initial parameters for best model (affine transform)
% Affine transform : T = [bp1, bp2, bp3, bp4; bp5, bp6, bp7, bp8; bp9, bp10, bp11, bp12;]
bp1 = bpI(1,1); bp2 = bpI(1,2); bp3 = bpI(1,3); bp4 = bpI(1,4);
bp5 = bpI(1,5); bp6 = bpI(1,6); bp7 = bpI(1,7); bp8 = bpI(1,8);
bp9 = bpI(1,9); bp10 = bpI(1,10); bp11 = bpI(1,11); bp12 = bpI(1,12);
for i=1:iter
% Randomly select 4 points
idx = randperm(number,num); sample = data(idx,:);
% Creating others which is the data that does not contain data in sample
idxs = sort(idx, 'descend'); % Sorting idx
others = data;
for n = 1:num
others(idxs(1,n), :) = [];
end
x1 = sample(1,1); y1 = sample(1,2); z1 = sample(1,3);
x2 = sample(2,1); y2 = sample(2,2); z2 = sample(2,3);
x3 = sample(3,1); y3 = sample(3,2); z3 = sample(3,3);
x4 = sample(4,1); y4 = sample(4,2); z4 = sample(4,3);
X1 = sample(1,4); Y1 = sample(1,5); Z1 = sample(1,6);
X2 = sample(2,4); Y2 = sample(2,5); Z2 = sample(2,6);
X3 = sample(3,4); Y3 = sample(3,5); Z3 = sample(3,6);
X4 = sample(4,4); Y4 = sample(4,5); Z4 = sample(4,6);
B = [X1; Y1; Z1; X2; Y2; Z2; X3; Y3; Z3; X4; Y4; Z4];
A = [
x1, y1, z1, 1, 0 , 0 , 0, 0, 0, 0, 0, 0;
0 , 0 , 0, 0, x1, y1, z1, 1, 0, 0 ,0, 0;
0 , 0 , 0, 0, 0 , 0 , 0, 0, x1, y1, z1, 1;
x2, y2, z1, 1, 0 , 0 , 0, 0, 0, 0, 0, 0;
0 , 0 , 0, 0, x2, y2, z2, 1, 0 , 0 ,0, 0;
0 , 0 , 0, 0, 0 , 0 , 0, 0, x2, y2, z2, 1;
x3, y3, z3, 1, 0 , 0 , 0, 0, 0, 0, 0, 0;
0 , 0 , 0, 0, x3, y3, z3, 1, 0 , 0 ,0, 0;
0 , 0 , 0, 0, 0 , 0 , 0, 0, x3, y3, z3, 1;
x4, y4, z4, 1, 0 , 0 , 0, 0, 0, 0, 0, 0;
0 , 0 , 0, 0, x4, y4, z4, 1, 0 , 0 ,0, 0;
0 , 0 , 0, 0, 0 , 0 , 0, 0, x4, y4, z4, 1
];
cbp = A\B; % calculating best parameters of the model (affine transform)
T = [reshape(cbp',[4,3])'; 0, 0, 0, 1]; % Current affine transform matrix
% Computing other points in the dataset that their distance from the
% calculated model is less than the threshold.
numOthers = size(others,1);
inliers = [];
for j = 1:numOthers
% b = T a
d = others(j,:); % Explanation: d = [ax, ay, az, bx, by, bz]
a = [d(1,1:3), 1]'; % Explanation a = [ax, ay, az]'
b = [d(1,4:6),1]'; % b = [bx, by, bz]'
cb = T*a; % Calculated b
dist = sum((cb-b).^2);
if(dist<=distThresh)
inliers = [inliers; d];
end
end
numinliers = size(inliers,1);
% Update the number of inliers and fitting model if better model is found
if (numinliers >= bestInNum)
% Better model is estimated
bestInNum = numinliers;
bp1 = cbp(1,1); bp2 = cbp(2,1); bp3 = cbp(3,1); bp4 = cbp(4,1);
bp5 = cbp(5,1); bp6 = cbp(6,1); bp7 = cbp(7,1); bp8 = cbp(8,1);
bp9 = cbp(9,1); bp10 = cbp(10,1); bp11 = cbp(11,1); bp12 = cbp(12,1);
bp = [bp1, bp2, bp3, bp4, bp5, bp6, bp7, bp8, bp9, bp10, bp11, bp12];
end
end
bp
end