I am trying to recreate a stereo rectification algorithm (without using any libraries).
I have calibrated a stereo camera rig using MATLAB. With MATLAB's built-in stereo rectification algorithm, I obtain usable rectified images.
My current implementation (which uses the same calibration results as the MATLAB algorithm) produces images that are slightly off (corresponding features are not horizontally aligned). For reference, a cropped anaglyph of the undistorted images is given.
Code for rectification of the left image:
load('calibration.mat');%load stereoParams
leftImages = imageDatastore('captures\left\');
distortedImage = readimage(leftImages, 1);
kL = stereoParams.CameraParameters1.IntrinsicMatrix';
k1 = stereoParams.CameraParameters1.RadialDistortion(1);
k2 = stereoParams.CameraParameters1.RadialDistortion(2);
translation = stereoParams.TranslationOfCamera2 .* [-1 -1 1]; %translation vector according to initial left camera frame
e1 = translation / norm(translation);
e2 = [-translation(2), translation(1), 0] / norm(translation(1:2));
e3 = cross(e1, e2);
transform = kL * [e1; e2; e3] / kL;
rectifiedImage(1 : 1000, 1 : 1000) = 0;
for r = 1 : 1000
for c = 1 : 1000
undistortedCoord = eye(3) / kL / transform * [c - 1; r - 1; 1]; %map the rectified coordinate to its corresponding unrectified coordinate
undistortedCoord = undistortedCoord / undistortedCoord(3); %normalize unrectified coordinate with respect to its homogeneous component
radius = sqrt(undistortedCoord(1) ^ 2 + undistortedCoord(2) ^ 2); %compute distance from image plane principal point
distortedCoord = [undistortedCoord(1:2) * (1 + k1 * radius ^ 2 + k2 * radius ^ 4); 1]; %compute distorted coordinate from lens distortion coefficients
imageCoord = kL * distortedCoord; %map the distorted coordinate into the distorted image
if (imageCoord(1) > 0 && imageCoord(2) > 0 && imageCoord(1) < size(distortedImage, 2) - 1 && imageCoord(2) < size(distortedImage, 1) - 1)
rectifiedImage(r, c) = double(distortedImage(round(imageCoord(2) + 1), round(imageCoord(1) + 1))) / 255;
end
end
end
imwrite(rectifiedImage, 'rectifiedL.png');
Code for rectification of the right image:
load('calibration.mat');%load stereoParams
rightImages = imageDatastore('captures\right\');
distortedImage = readimage(rightImages, 1);
kL = stereoParams.CameraParameters1.IntrinsicMatrix';
kR = stereoParams.CameraParameters2.IntrinsicMatrix';
k1 = stereoParams.CameraParameters2.RadialDistortion(1);
k2 = stereoParams.CameraParameters2.RadialDistortion(2);
translation = stereoParams.TranslationOfCamera2 .* [-1 -1 1]; %translation vector according to initial left camera frame
e1 = translation / norm(translation);
e2 = [-translation(2), translation(1), 0] / norm(translation(1:2));
e3 = cross(e1, e2);
transform = kL * [e1; e2; e3] * stereoParams.RotationOfCamera2 / kR;
rectifiedImage(1 : 1000, 1 : 1000) = 0;
for r = 1 : 1000
for c = 1 : 1000
undistortedCoord = eye(3) / kR / transform * [c - 1; r - 1; 1]; %map the rectified coordinate to its corresponding unrectified coordinate
undistortedCoord = undistortedCoord / undistortedCoord(3); %normalize unrectified coordinate with respect to its homogeneous component
radius = sqrt(undistortedCoord(1) ^ 2 + undistortedCoord(2) ^ 2); %compute distance from image plane principal point
distortedCoord = [undistortedCoord(1:2) * (1 + k1 * radius ^ 2 + k2 * radius ^ 4); 1]; %compute distorted coordinate from lens distortion coefficients
imageCoord = kR * distortedCoord; %map the distorted coordinate into the distorted image
if (imageCoord(1) > 0 && imageCoord(2) > 0 && imageCoord(1) < size(distortedImage, 2) - 1 && imageCoord(2) < size(distortedImage, 1) - 1)
rectifiedImage(r, c) = double(distortedImage(round(imageCoord(2) + 1), round(imageCoord(1) + 1))) / 255;
end
end
end
imwrite(rectifiedImage, 'rectifiedR.png');
Besides the homographies applied, these blocks of code are identical.
MATLAB's stereo rectification algorithm is based on Bouguet's Algorithm, while mine is based on a slightly different algorithm, so I do not expect identical results, still I believe I should be getting valid rectified images.
I have tried number of variations (negative translation vectors, transposed rotation matrices, different output intrinsic matrices, different orders of matrix multiplication, etc.), but I am unable to produce any valid results. According to the algorithm I am using, I should reverse the order of rotation matrix multiplication, but doing this only shifts the right image vertically, therefore increasing misalignment. I initially though that the images were at slightly different scales, but surely this is not possible when using the same output intrinsic matrix across both images. The translation vector has been element-wise multiplied by [-1 -1 1] to match the left camera coordinate frame that shows in MATLAB's calibration tool. I have tried other variants, such as [-1 -1 -1], [1 1 1], etc. which can give similar results if certain matrices are transposed, however, none give valid rectification.
When I use the homographies produced internally by MATLAB, I get valid results, what is wrong with my homographies?
It turns out that the problem was caused by the fact that the translation vector was initially described in terms of the second camera coordinate frame, so changing the translation vector assignment to
translation = (-stereoParams.RotationOfCamera2 * stereoParams.TranslationOfCamera2')';
results in valid rectification.
In hindsight, I should have tested this a long time ago, but the translation seemed to correlate roughly with what I expected in the first camera's coordinate frame, if not for the signs of the individual components.
Its very frustrating, because MATLAB's documentation suggests that the translation is already taken with respect to the first camera's coordinate frame.