Working PS Move plugin for UE4; need help coreg with DK2

I've badly read the Shiu 1989 paper, it doesn't say that RA is equal to RB, only that the angle of rotation is equal, but the axis is not equal.

There are at least two algorithms for the AX=XB problem that don't require to know RB (rotational part of B). The more recent one seems to require the knowledge of an initial pose for B (rotation + translation), so it's probably not usable.

The older one requires a good initial estimate of X, but I guess (0,0,0) can be used when the relative orientation of the PS Eye and the DK2 camera is similar, which should generally be the case.

Here is the paper :
- A New Identification Jacobian for Robotic Hand/Eye Calibration (Zhuang 1994)

If anyone has any idea about how to implement this in MATLAB, feel free to answer.

I'm sorry I don't have a full understanding of the problem you're facing.

But it seems like the rotation value of the PS Move's are not accurate?

Would you be able to get a more accurate rotational value of the PS Move's if you somehow made use of 2 Move controllers attached to each other? Like 2 controllers taped to each other, then attached to the DK2?

Maybe you can derive rotational value since there are 2 optical points tracked?

Sorry if this is not relevant.

Keep up the good work. I've been watching this thread for a while. I'm looking to develop for HTC Vive, and these controllers might be useful in the time until the consumer Vive's are released at the end of the year.

The tilt (pitch + roll) are pretty good. It's just the yaw that's the problem. This can be improved by tweaks to the IMU sensor fusion algorithm. One necessary step would be to ask the user to keep the controller still to calibrate the gyroscope noise. And there are other things that can be done with the magnetometer readings to make them more reliable. I will revisit this in the near future.

If anyone else wants to take a stab at this, I think a good place to start would be with the moveframework orientation code and reading the posts by Dave LaValle on the Oculus blog.

That being said, fixing the orientation won't really help the co-registration problem by itself because the PSEye camera has no way of knowing the controller orientation in a useful reference frame. With the DK2, the camera sees multiple LEDs with known separation and thus can determine the DK2's position and orientation in camera space, and working back from the DK2 IMUs that give orientation in gravity-space, can figure out the camera's orientation in gravity-space. This is not possible with the PSMove.

What you suggested about using the two PSMove controllers simultaneously is interesting for the coregistration problem but is orthogonal to the orientation problem. Our new student had a similar idea using the little red LED at the bottom of the controller but I dismissed this because I don't think you could get a reliable size estimate from the tiny light. If the two controllers were fixed together with known transforms then every time the camera sampled both locations it could draw a vector connecting the two spheres then combine that with each controller's orientation (in gravity space) and known transformations (position in camera space, position and orientation in gravity space) to get the camera orientation in gravity space. However, we're still faced with the problem that the orientation data coming off the two controllers are not very reliable. And I'm skeptical that you could rely on users to fasten the two controllers together reliably.

We're trying to find a solution with minimal external hardware. If you could ask all users to get external hardware then finding the camera-camera transform would be trivial with an object with 3 broadband LEDs (or 6 LEDs -- 3-IR and 3-visible -- could also work) that both cameras picked up.

"bullale" wrote:
Our new student had a similar idea using the little red LED at the bottom of the controller but I dismissed this because I don't think you could get a reliable size estimate from the tiny light.

That's quite smart, I didn't think about this. Since we know that in axis-angle representation the PS Move and the DK2 have the same angle, maybe using the red LED and the PS Move orientation with the orb position we can get a reasonably good value for the axis of the PS Move rotation.

I'll try to implement the Zhuang paper first though, I'll try to decompose the steps and post them here.

From another paper from Zhuang it seems that their algorithm can't in fact find the transformation matrix X from AX=XB without knowing the orientation of B :

In this correspondence, we point out that although the iterative algorithm does not require direct measurement of the sensor relative rotation, it still needs the user to provide the same amount of sensor orientation information.

Source : A note on using only position equations for robotic hand/eye calibration.

I don't have access to the paper unfortunately to confirm this, if anyone has access to it, feel free to comment.

Also two other papers from Zhuang could be interesting as well but they are not freely available, although the algorithm much probably suffers from the same problem :
- A noise-tolerant algorithm for robotic hand-eye calibration with or without sensor orientation measurement
- A Noise Tolerant Algorithm For Wrist-mounted Robotic Sensor Calibration With Or Without Sensor Orientation Measurement

I fear that there is no solution to the PS Move/DK2 problem without knowing the PS Move orientation in the PS Eye space referential.

"bullale" wrote:
Our new student had a similar idea using the little red LED at the bottom of the controller but I dismissed this because I don't think you could get a reliable size estimate from the tiny light.

I think you don't necessarily need a reliable size estimate from the tiny light. You can measure the physical distance between the red light and the orb (and calculate the distance between the red light and the center of the orb) and you know the distance of the orb, so I think you can infer the position of the red light and the orientation of the PS Move on at least 2 axis.

I'm not sure if it's possible to get a usable orientation without knowing the 3rd axis or if there is a way to calculate it, knowing that the angle is equal to the one of the DK2 orientation.

Since resolving the AX = XB problem seems to be a dead end without PS Move orientation data, I've re-read the Ernst 2012 paper more in depth (dealing with the AX = YB problem).

I think I understand how it does work, but it doesn't fully solve the problem. It can solve Y fully (the translation and orientation between the PS Move and the DK2) but only the translational part of X (the transform between the PS Eye and the DK2 camera), whereas the rotational part is also needed.

I'll try to implement it in MATLAB and in C# in Unity anyway. Maybe it's possible to get the rotational part of X using these calculated values and another algorithm based on absolute orientation (Horn's method).

I've read several papers from Zhuang that someone sent me (thanks!) but they don't answer the problem. They solve the AX=XB equation but B in this case corresponds to a combination of two poses of the PS Move. The rotation part is not needed in the final equation, but it's needed to build B, so it seems to be a dead-end.

I've read a bit more about the AX=YB equation and it seems I incorrectly thought that solving X was required when in fact it's Y that corresponds to the transform between the two cameras in the AX=YB equation. The Ernst 2012 QR15 algorithm seems to be a good fit for the problem then, so I've tried to implement it from what is described in the paper (very briefly).

Unfortunately I don't get back the original results, here is my MATLAB code if anyone can shed a light on this :

% QR15 calibration algorithm (AX = YB without rotational information from the tracking system)
% Non-orthogonal tool/flange and robot/world calibration - Ernst 2012

system cls;

function mat=rigidmat(tx, ty, tz, rx, ry, rz)
  rx = rx * pi / 180;
  ry = ry * pi / 180;
  rz = rz * pi / 180;

  mat = [
    cos(ry)*cos(rz), cos(rx)*sin(rz)+sin(rx)*sin(ry)*cos(rz),sin(rx)*sin(rz)-cos(rx)*sin(ry)*cos(rz), tx;
    -cos(ry)*sin(rz), cos(rx)*cos(rz)-sin(rx)*sin(ry)*sin(rz),sin(rx)*cos(rz)+cos(rx)*sin(ry)*sin(rz), ty;
    sin(ry), -sin(rx)*cos(ry), cos(rx)*cos(ry), tz
    0,0,0,1
  ];
end

function [x,y,z] = mat2euler(m)
	x = atan2(m(3,2), m(3,3));
	y = atan2(-m(3,1), sqrt(m(3,2) * m(3,2) + m(3,3) * m(3,3)));
	z = atan2(m(2,1), m(1,1));

  x = -(x * 180) / pi;
  y = -(y * 180) / pi;
  z = -(z * 180) / pi;
end

function display(v)
  printf("\n%s:\n", inputname(1));
  printf([repmat('% .4f ', 1, size(v, 2)) '\n'], v');
end

% Create 2 transform matrices
X = rigidmat(1, 2, 3, 10, 5, 2);
display(X);
[rx,ry,rz] = mat2euler(X);
printf("\nRotation:% .2f % .2f % .2f\n", rx, ry, rz);

Y = rigidmat(3, 2.1, 2.9, 12, 7, 3);
display(Y);
[rx,ry,rz] = mat2euler(Y);
printf("\nRotation:% .2f % .2f % .2f\n", rx, ry, rz);

% Create AA and BB matrices
npoints = 250;
AA = zeros(4, npoints*4);
BB = zeros(4, npoints*4);

%pos = 0.25;
pos = 1.0;
%angle = 10;
angle = 30;
for i = 1:npoints
  p = rigidmat(
    pos*(2*rand-1), pos*(2*rand-1), pos*(2*rand-1),
    angle*(2*rand-1), angle*(2*rand-1), angle*(2*rand-1));
  AA(1:4,4*i-3:4*i) = p;
  BB(1:4,4*i-3:4*i) = inv(Y) * p * X;
end

[m,n]=size(AA); n = n/4;

printf("\nNumber of elements: %d\n", n);
%display(AA);
%display(BB);

% Aw = b
%
% A : [A1 A2 ... An]T : 3n x 15
% b : [b1 b2 ... bn] : 3n
% w : [x11 x21 ... x34 y11 y21 ... y34]T : 15 ?
%
% Ai = [R[Mi](Ni)14 R[Mi](Ni)24 R[Mi](Ni)34 R[Mi] - E3]
% bi = [-T[Mi]]
%
% => Y and T[X]

A = zeros(3*n, 15);
b = zeros(3*n, 1);

for i = 1:npoints
  RMi = AA(1:3,4*i-3:4*i-1);
  A(3*i-2:3*i,1:15) = [RMi*BB(1:1,4*i:4*i) RMi*BB(2:2,4*i:4*i) RMi*BB(3:3,4*i:4*i) RMi -eye(3)];
  b(3*i-2:3*i) = -AA(1:3,4*i:4*i);
end

%display(A);
%display(b);

%x = zeros(15, 1);
%x = lsqr(A, b, 0, 1000);
%display(x);

x = zeros(15, 1);
x = lsqrSOL(3*n, 15, A, b, 0, 0, 0, 0, 1000, 0);
display(x);

The lsqr and lsqrSOL functions are implementations I've found of the lsqr function in MATLAB (least squares QR factorization) which isn't available in Octave. Both functions give the same result with the same data so the problem doesn't seem to be here.

The problem is most probably in the way I build the A and b matrices, or maybe there is not enough information in the paper to implement the algorithm.

The problem is in the algorithm. When I build the test data, if instead of using B = inv(Y)*A*X I use B = inv(Y)*inv(A)*X I can obtain the correct X (translation only) and Y matrices.

So I've modified the algorithm to use the inverse of A in the calculation, using this as a reference (inverse of [R|t] is [transpose(R) |-transpose(R).t]).

The algorithm was solving Aw=b using least squares QR factorization (w containing the elements of X and Y) with each element of A and b constructed this way :
- Ai = [RATB1 RATB2 RATB3 RA -E3]
- bi = -TA

I replaced it with these elements :
- Ai = [transpose(RA)TB1 transpose(RA)TB2 transpose(Ra)TB3 transpose(RA) -E3]
- bi = RaTA

And I get the correct X and Y matrices. You can't imagine how happy I'm after having spent 2 months trying to find a solution to this problem. :)

Now I need to implement that in C#, which isn't going to be a simple task considering the complexity of the lsqr function, but at least I've two implementations in MATLAB that I can study.

Not sure if anyone is still following, here is my progress anyway.

I've implemented the Matrix4x4.TRS and Quaternion.Euler Unity functions in MATLAB in order to have the exact same use cases in both environments. When I use these functions in Octave to create the test data (X, Y, A and B transformation matrices) I still get the correct X and Y matrices in the end.

I've found a lsqr function callable from C#, which is a C++ implementation of the same MATLAB function I use in Octave, which I've recompiled and I can call in Unity in 32 or 64 bits.

I've verified at each point that I've exactly the same values in Octave and Unity, but unfortunately the two lsqr functions don't return the same result. So I guess there must be a difference between the implementations and as I feared I'll probably have to implement the lsqr function in C# myself.

That is, if nobody knows an existing C# implementation.