Mobile robot pose estimation

Question

I want to plot the path of a vehicle via the estimation of egomotion based on essential matrix.

Everything was fine with openCV and the following function.

function [ xnow] = estimate_pose_test( points1, points2, K,xLast )


%%%%OpenCV%%%%%

  E = cv.findEssentialMat(points1, points2, 'CameraMatrix',K, 'Method','Ransac');
 [R, t] = cv.recoverPose(E, points1, points2,'CameraMatrix',K);
 ry= asin(R(1,3));
 u=[t(1,1);t(3,1);ry];

%%%%%%%%%Opengv%%%%%%
% for i=1:size(points1,2)
%     I1(:,i)=points1{i}';
%     I2(:,i)=points2{i}';
% end
%  temp = K \ [I1; ones(1,size(I1,2))];
%  I1_norms = sqrt(sum(temp.*temp));
%  I1n = temp ./ repmat(I1_norms,3,1);
%  
%  temp = K \ [I2; ones(1,size(I2,2))];
%  I2_norms = sqrt(sum(temp.*temp));
%  I2n = temp ./ repmat(I2_norms,3,1);
%  
%  X = opengv('fivept_nister_ransac',I1n,I2n);
%  R = X(:,1:3);
%  t = X(:,4);
%  ry= asin(R(1,3));
%  u=[t(1,1);t(3,1);ry];
%  


 theta=xLast(3)+u(3);

 if(theta>pi)
    theta=theta-2*pi;
elseif(theta<-pi)
    theta = theta+2*pi;
 end

s = sin(xLast(3));
c = cos(xLast(3));
% actual value added with the new control vector

xnow=[xLast(1:2)+[c s; -s c]*u(1:2);theta];




end

points1 and points2 are corresponding SURF features. K is internal calibration matrix.

However, I want to use OpenGV librairy. OpenGV expects normalized coordinates on the unit sphere, so I started by transforming the measurements as recommended in the previous link and shown in the commented part in the above function.

The plotted path was totally wrong and the results between Opencv and Opengv are different.

For example for the same two consecutive frames, from opencv, I obtained the following rotation and translation:

R1 =

    0.9999    0.0016   -0.0153
   -0.0017    1.0000   -0.0054
    0.0153    0.0055    0.9999

t1 =

    0.1159
    0.1042
   -0.9878

And with OpenGv,

R2 =

    0.9998   -0.0059   -0.0167
    0.0060    1.0000    0.0050
    0.0166   -0.0051    0.9998

t2 =

    0.2776
   -0.0771
    0.6458

There is not even a constant scale factor between t2 and t1. Where is the problem? in plotting the resuts or in the estimation itself?

Edit I know that the function I wrote dosen't really make sense but it is just a test function to illustrate the problem I faced with OpenGV. First, what I want to do is to track the state of a vehicle defined by a reduced state vector $q_k=(x,z,\theta)$ in order to estimate, afterwards, the uncertainty on the position, because I'm working with a probabilistic approach where: $q_{k|k-1}=f(q_{k|k-1},u_{k-1})$ So, I need the values of control vector $u$ that's why I'm deriving $dx$, $dz$ and $d\theta$ from the outputted $R$ and $t$. Then, I'm plotting the path from the control vector values just to verify that i'm deriving them correctly by comparing the plotted path to the ground truth. Regarding the coordinate reference system, I'm using the following definition: so the vehicle is moving in the $XZ$ plane and the rotation is around the $Y$ axis that's why I'm using $t(1), t(3)$ as position and $ry$ as rotation in the the control vector.

Answer 1

I'm having a really hard time understanding what you're doing or why, so I'm going to try to break down your code as I understand it, pointing out along the way why I think what you're doing doesn't make sense. This isn't meant to be demeaning or anything, it's my problem in that I haven't done enough (any) CV work to make sense of it.

Here goes:

%%%%OpenCV%%%%%`

E = cv.findEssentialMat(points1, points2, 'CameraMatrix',K, 'Method','Ransac');
[R, t] = cv.recoverPose(E, points1, points2,'CameraMatrix',K);

Okay, great - give some key points between two frames and see how they shift, then estimate your (change in) position given those points and the camera matrix. I understand this.

ry= asin(R(1,3));
u=[t(1,1);t(3,1);ry];

Okay you lost me. You are given a perfectly good rotation matrix R, and then... what? You're trying to find the rotation about the y-axis? It looks like you're trying to use the definition of a rotation matrix, given on the Wikipedia page as:

$$ R_y(\theta) = \left[\begin{array}{ccc} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{array}\right] $$

But, the problem here is those zeros - you don't have them in your output $R$ matrix. You do not have a matrix that is strictly a rotation about the $y$ axis. What's more is the fact that you said,

I want to estimate the pose of a robot defined by (x,y,theta).

So, what rotation are you expecting? If you rotate about the $y$ axis, then what you're saying is that you want to rotate points in the x-z plane. But, if you don't have any z-axis points, then all you're doing is rotating the x-axis points and you're not rotating the y-axis points at all. Verify this by looking at the second row of that matrix definition: $\left[\begin{array}{ccc} 0&1&0 \\ \end{array}\right]$ - it exactly preserves the second row (y-values) of your points matrix.

Maybe this is what you want though, because the points you keep from the translation vector are the first and third, the $x$ and $z$ values. But again, you said you want to keep the $x$ and $y$ values, and this is where my lack of understanding of your inputs is hurting me. Maybe you are providing the inputs points1 and points2 as a 3xn matrix of points, [x;0;z], and everything is processing correctly. However, I did note that you can also pass a 2xn matrix of points, so again I'm not sure what your data set is or how you're expecting the output to look.

But beyond that, I'll skip the OpenGV section as it's commented out and go to:

theta=xLast(3)+u(3);

So here it looks like you're attempting a successive rotation by adding the previous rotation angle ry, from xLast, to the current value of rotation, ry.

(Side note here - I would call the rotation something like dTheta to show it's a difference value, like dx/dt, for example, but that's a nit-picky kind of point.)

if(theta>pi)
  theta=theta-2*pi;
elseif(theta<-pi)
  theta = theta+2*pi;
end

Bounds checking, okay.

s = sin(xLast(3));
c = cos(xLast(3));

Setting up for... a rotation matrix??

% actual value added with the new control vector
xnow=[xLast(1:2)+[c s; -s c]*u(1:2);theta];

And now mashing up the new rotation matrix with the previous information.

I mentioned the term "successive rotations" a bit ago, but you should read more about the homogeneous transform (PDF). Basically, you're allowed to do successive transforms just by multiplying the two together.

First, compose the matrix:

$$ H = \left[\begin{array}{ccc} R & t \\ \left[\begin{array}{ccc} 0 & 0 & 0 \\ \end{array}\right] & 1 \\ \end{array}\right] $$ Any time you get a new update, just post-multiply the new matrix. So, for example, on your first iteration, you'll get the pose of the second frame relative to the first:

$$ ^1H_2 = \left[\begin{array}{ccc} ^1R_2 & ^1t_2 \\ \left[\begin{array}{ccc} 0 & 0 & 0 \\ \end{array}\right] & 1 \\ \end{array}\right] $$

Your second iteration will give you the third frame relative to the second:

$$ ^2H_3 = \left[\begin{array}{ccc} ^2R_3 & ^2t_3 \\ \left[\begin{array}{ccc} 0 & 0 & 0 \\ \end{array}\right] & 1 \\ \end{array}\right] $$

BUT, what you really care about is the third frame relative to the first, so just multiply the two transforms together:

$$ ^1H_3 = (^1H_2)(^2H_3) \\ $$

Want to get the fourth frame relative to the first? Easy -

$$ ^1H_4 = (^1H_3)(^3H_4) \\ $$

And so on. You'll get:

$$ ^1H_n = (^1H_{n-1})(^{n-1}H_n) \\ $$

The incremental adjustment, $^{n-1}H_n$, is the output of each iteration because you get $^{n-1}R_n$, the incremental rotation, and $^{n-1}t_n$, the incremental translation.

All this is to say that (I think) you should stop re-inventing the wheel. You dissolve a rotation matrix into an angle just to re-create a rotation matrix later. If you want to get the rotation matrix and translation vector at the end, just pull the appropriate parts: R = H(1:3,1:3); and t = H(1:3,4);. If you want, you can pull the individual components dX = t(1);, dY = t(2);, and dZ = t(3);. I use the delta variable names dX, etc. as a reminder that these are all relative translations from the first frame.

Regarding orientation, you should really be looking at a more proper conversion of the rotation matrix. You could use Euler angles or quaternions, but you really should not be trying to pull a single rotation angle from a single value in a matrix.

So, if I had your code to write again, I would probably go:

function [ xnow] = estimate_pose_test( points1, points2, K,xLast )
%%%%OpenCV%%%%%

E = cv.findEssentialMat(points1, points2, 'CameraMatrix',K, 'Method','Ransac');
[R, t] = cv.recoverPose(E, points1, points2,'CameraMatrix',K);
xnow = xLast*[R,t; zeros(1,3),1];

This keeps xnow and xLast as the 4x4 transform, but again if you really wanted the x/y/z values they're just the 3x1 vector starting at (1,4) in the homogeneous transform. Also, as mentioned, you should really be using a different method of displaying the rotation angle. You can keep the rotation matrix for internal stuff, but maybe Euler angles or Tait-Bryan angles would be the best for displaying a value to a user.

PS - I would guess the X you get from the OpenGV code is actually the full homogeneous transform matrix, so you save a step by not needing to make it yourself (not that it's hard).