Hello. I was testing mexopencv functions for estimating pose for an image pair from MATLAB's Visual Odometry Example. However, the result of relative location computed by mexopencv functions is different (negative sign) from the ground-truth. Moreover, the results computed from MATLAB's Functions are matching with the ground-truth.
I have observed that this type of discrepancy is generated when there is a forward movement in the camera (shown in the images).
Why is it so? Is there any underlying bug in the code? Both codes are provided below.
MATLAB's Code:
close all, clear all, clc
K = [615 0 320; 0 615 240; 0 0 1];
cameraParams = cameraParameters('IntrinsicMatrix', K);
images = imageDatastore(fullfile(toolboxdir('vision'),'visiondata','NewTsukuba'));
% Load ground truth camera poses.
load(fullfile(toolboxdir('vision'),'visiondata','visualOdometryGroundTruth.mat'));
viewId = 15; % Number of 2nd Image, in the image pair that is to be matched
Irgb = readimage(images, viewId-1); % Read image number 1
% Convert to gray scale and undistort.
prevI = undistortImage(rgb2gray(Irgb), cameraParams);
prevPoints = detectSURFFeatures(prevI, 'MetricThreshold', 500); % Detect features.
% Select a subset of features, uniformly distributed throughout the image.
numPoints = 150;
prevPoints = selectUniform(prevPoints, numPoints, size(prevI));
% Extract features. Using 'Upright' features improves matching quality if
% the camera motion involves little or no in-plane rotation.
prevFeatures = extractFeatures(prevI, prevPoints, 'Upright', true);
% Read image 2.
Irgb = readimage(images, viewId);
% Convert to gray scale and undistort.
I = undistortImage(rgb2gray(Irgb), cameraParams);
% Match features between the previous and the current image.
[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(prevFeatures, I);
format long
% Estimate the pose of the current view relative to the previous view.
[relative_orient, relative_location, inlierIdx] = helperEstimateRelativePose(...
prevPoints(indexPairs(:,1)), currPoints(indexPairs(:,2)), cameraParams);
relative_location_normalized = relative_location * norm(groundTruthPoses.Location{viewId});
groundTruth_Loc = groundTruthPoses.Location{viewId};
display(relative_location_normalized);
display(groundTruth_Loc);
MEXOPENCV's Code:
close all, clear all, clc
images = imageDatastore(fullfile(toolboxdir('vision'),'visiondata','NewTsukuba'));
viewId = 15; % Number of 2nd Image, in the image pair that is to be matched
im1 = readimage(images, viewId-1);
im2 = readimage(images, viewId);
load(fullfile(toolboxdir('vision'),'visiondata','visualOdometryGroundTruth.mat'));
detector = cv.SIFT('ConstrastThreshold',0.04);
matcher = cv.DescriptorMatcher('BruteForce-L1');
[k1,d1] = detector.detectAndCompute(im1);
[k2,d2] = detector.detectAndCompute(im2);
matches = matcher.knnMatch(d1, d2, 2);
idx = cellfun(@(matches) matches(1).distance < 0.7 * matches(2).distance, matches);
matches = cellfun(@(matches) matches(1), matches(idx));
ptsObj = cat(1, k1([matches.queryIdx]+1).pt); % queryIdx: index of query descriptors in image 1
ptsScene = cat(1, k2([matches.trainIdx]+1).pt); % trainIdx: index of train descriptors in image 2
cameraMatrix_VOdometry = [615 0 320; 0 615 240; 0 0 1]; % INTRINSIC MATRIX for NewTsukuba VO Dataset
format long
[E, mask] = cv.findEssentialMat(ptsObj,ptsScene,'CameraMatrix',cameraMatrix_VOdometry,'Method','LMedS');
%display(E);
%Recover relative camera rotation and translation from an estimated essential matrix
[relativeOrient, relativeLoc, good, inlierIdx] = cv.recoverPose(E, ptsObj, ptsScene,'Mask',mask); % points1 and points2 are the matched feature points (inliers)
groundTruth_Orient = groundTruthPoses.Orientation{viewId};
%display(groundTruth_Orient);
%display(relativeOrient);
relativeLoc = transpose(relativeLoc);
relativeLoc_normalized = relativeLoc * norm(groundTruthPoses.Location{viewId});
groundTruth_Loc = groundTruthPoses.Location{viewId};
display(groundTruth_Loc);
display(relativeLoc_normalized);