Why Pose Estimation Results of MEXOPENCV and MATLAB Functions so diverse?

asked 2018-04-05 14:08:02 -0500

SAKhan gravatar image

updated 2018-04-06 14:29:27 -0500

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);
edit retag flag offensive close merge delete

Comments

mexopencv is an opencv wrapper. you should ask https://github.com/kyamagu/mexopencv/...

LBerger gravatar imageLBerger ( 2018-04-05 14:26:37 -0500 )edit

They are saying to ask this question at OPENCV's forum... Where should I ask these questions? I am surprised that there is nobody who could help positively.

SAKhan gravatar imageSAKhan ( 2018-04-05 14:37:54 -0500 )edit

In my opinion, if you want to debug you should:

  • identify where the discrepancy could come from
  • skip all the features part: load raw data in order to have the same input for Matlab::estimateEssentialMatrix and OpenCV::findEssentialMat
  • if the estimated essential matrices are similar/correct, check the output of Matlab::relativeCameraPose and OpenCV::recoverPose by feeding them with the same input
Eduardo gravatar imageEduardo ( 2018-04-06 04:53:22 -0500 )edit

I did these steps. But following results are generated by the above provided 2 codes:

groundTruth_Loc =  -2.191584000000000  -0.018982000000000  26.857834000000000

relativeLoc_normalized = -0.065220918814029  -0.049103970994921  26.946984672662680 % MATLAB's Result

relativeLoc_normalized = 2.487593988338816  3.514638449996284  -26.600861652057898 % MEXOPENCV's Result

Why MEXOPENCV's Relative Location Vector has opposite sign for its 3rd element? (as compared to ground-truth and MATLAB's Result)

SAKhan gravatar imageSAKhan ( 2018-04-06 14:39:22 -0500 )edit

So you did check that both versions output similar results relative_orient, relativeLoc? The results from recoverPose and helperEstimateRelativePose are the same if you feed the same input?

And the issue should come these lines?

  • relative_location_normalized = relative_location * norm(groundTruthPoses.Location{viewId});
  • relativeLoc = transpose(relativeLoc);
  • relativeLoc_normalized = relativeLoc * norm(groundTruthPoses.Location{viewId});
Eduardo gravatar imageEduardo ( 2018-04-07 08:39:35 -0500 )edit

No... The discrepancy arises in the estimation of Essential Matrix and subsequently in Pose estimation from the computed Essential Matrix... Following functions are involved in the estimation of Essential Matrix:

MATLAB:

[relative_orient, relative_location, inlierIdx] = helperEstimateRelativePose(prevPoints(indexPairs(:,1)), currPoints(indexPairs(:,2)), cameraParams);

MEXOPENCV:

[E, mask] = cv.findEssentialMat(ptsObj,ptsScene,'CameraMatrix',cameraMatrix_VOdometry,'Method','LMedS');
[relativeOrient, relativeLoc, good, inlierIdx] = cv.recoverPose(E, ptsObj, ptsScene,'Mask',mask);
SAKhan gravatar imageSAKhan ( 2018-04-08 06:07:03 -0500 )edit

@Eduardo can you help?

SAKhan gravatar imageSAKhan ( 2018-04-09 14:08:05 -0500 )edit