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

I've over 10 years experience with Matlab. Let me know if there's anything I can do to help.

This might be a bit naive, but does it matter whether the coordinate systems used by the psmoveapi is left-handed or right-handed? Currently its positional coordinate system is right-handed. I will have to double-check the rotational coordinate system.

"bullale" wrote:
I've over 10 years experience with Matlab. Let me know if there's anything I can do to help.

Ah nice, I'll do in case of problems. Thanks.

"bullale" wrote:
This might be a bit naive, but does it matter whether the coordinate systems used by the psmoveapi is left-handed or right-handed? Currently its positional coordinate system is right-handed. I will have to double-check the rotational coordinate system.

No idea yet. I remember I had to multiply the PS Move orientation with a specific angle previously but maybe the algorithm takes care of that automatically. I'll let you know if that's the case.

I'm getting more familiar with MATLAB and I've added some code to test the validity of the algorithm:
- create two arbitrary transformation matrices (from PS Eye to DK2 camera and from PS Move to DK2)
- generate several hundreds random DK2 positions (rotation+translation)
- transform the DK2 positions into PS Move positions using the transform matrices
- run the algorithm

It gives back the original transform matrices, so all is well and I can use this as a test case to validate the C# implementation.

I don't get back the original Euler angles but since the matrices are the same it's not that important for now. I guess the transformation of a rotation matrix into Euler angles is not necessarily unique, maybe related to the gimbal lock problem.

Here is the modified MATLAB code:

system cls;

function mat=rigidmat(tx, ty, tz, rx, ry, rz)
  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 [s,c,r,t]=atan2sc(y,x)
  t=NaN;
  if y == 0
      t=(x<0);
      c=1-2*t;
      s=0;
      r=abs(x);
      t=t*pi;
  elseif x == 0
      s=2*(y>=0)-1;
      c=0;
      r=abs(y);
      t=s*0.5*pi;
  elseif (abs(y) > abs(x))
      q = x/y;
      u = sqrt(1+q^2)*(2*(y>=0)-1);
      s = 1/u;
      c = s*q;
      r = y*u;
  else
      q = y/x;
      u = sqrt(1+q^2)*(2*(x>=0)-1);
      c = 1/u;
      s = c*q;
      r = x*u;
  end
  if nargout>3 && isnan(t)
      t=atan2(s,c);
  end
endfunction

function e=rotro2eu(m,ro)
  e=zeros(3,1);
  if ischar(m)
      m=lower(m)-'w';
  end
  if numel(m)~=3 | any(abs(m-2)>1), error('Euler axis must be an x,y or z triplet'); end
  u=m(1);
  v=m(2);
  w=m(3);
  if sum(m==v)>1, error('Consecutive Euler axes must differ'); end
  % first we rotate around w to null element (v,u) with respect to element (!vw,u) of rotation matrix
  g=2*mod(u-v,3)-3;   % +1 if v follows u or -1 if u follows v
  h=2*mod(v-w,3)-3;   % +1 if w follows v or -1 if v follows w
  [s,c,r,e(3)]=atan2sc(h*ro(v,u),ro(6-v-w,u));
  r2=ro;
  ix=1+mod(w+(0:1),3);
  r2(ix,:)=[c s; -s c]*ro(ix,:);
  % next we rotate around v to null element (!uv,u) with repect to element (u,u) of rotation matrix
  e(2)=atan2(-g*r2(6-u-v,u),r2(u,u));
  % finally we rotate around u to null element (v,!uv) with respect to element (!uv,!uv) = element (v,v)
  e(1)=atan2(-g*r2(v,6-u-v),r2(v,v));
  if (u==w && e(2)<0) || (u~=w && abs(e(2))>pi/2)  % remove redundancy
      mk=u~=w;
      e(2)=(2*mk-1)*e(2);
      e=e-((2*(e>0)-1) .* [1; mk; 1])*pi;
  end
  e(1) = (e(1) * 180) / pi;
  e(2) = (e(2) * 180) / pi;
  e(3) = (e(3) * 180) / pi;
endfunction

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

% Create 2 transform matrices
T1 = rigidmat(1, 2, 3, 10, 5, 2);
display(T1);
printf("\nRotation (deg): 10, 5, 2\n");

T2 = rigidmat(3, 2.1, 2.9, 12, 7, 3);
display(T2);
printf("\nRotation (deg): 12, 7, 3\n");

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

pos = 0.25;
ang = 10;
for i = 1:npoints
  p = rigidmat(
    pos*(2*rand-1), pos*(2*rand-1), pos*(2*rand-1),
    ang*(2*rand-1), ang*(2*rand-1), ang*(2*rand-1));
  AA(1:4,4*i-3:4*i) = p;
  BB(1:4,4*i-3:4*i) = inv(T2) * p * T1;
end

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

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

%display(AA);
%display(BB);

A = zeros(12*n, 24);
b = zeros(12*n, 1);

for i = 1:n
%printf("\nIteration %d\n", i);
    Ra = AA(1:3,4*i-3:4*i-1);
%display(Ra);
    Rb = BB(1:3,4*i-3:4*i-1);
%display(Rb);
    ta = AA(1:3,4*i);
%display(ta);
    tb = BB(1:3,4*i);
%display(tb);
    A(12*i-11:12*i-3,1:18) = [kron(Ra,eye(3)) kron(-eye(3),Rb')];
    A(12*i-2:12*i,10:24) = [kron(eye(3),tb') -Ra eye(3)];
    b(12*i-2:12*i) = ta;
end

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

x = A\b;

%display(x);

X=reshape(x(1:9),3,3)';
[u,s,v]=svd(X); X = u*v'; if det(X)<0, X = u*diag([1 1 -1])*v'; end
X = [X x(19:21);[0 0 0 1]];

display(X);
[rx,ry,rz] = mat2euler(X(1:3,1:3));
e = rotro2eu("xyz", X(1:3,1:3));
%printf("\nRotation: % .2f % .2f % .2f\n", rx, ry, rz);
%display(e);

Y=reshape(x(10:18),3,3)';
[u,s,v]=svd(Y); Y = u*v'; if det(Y)<0, Y = u*diag([1 1 -1])*v'; end
Y = [Y x(22:24);[0 0 0 1]];

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

The algorithm seems to work in the same way in C# than in MATLAB.

I've tested it with the same test data but with less precise values and I get very similar rotation matrices, but the translation vectors are way off. I don't know if it's inherent to the algorithm which is known to have large positional errors, because I used QR decomposition to replace the \ operator in MATLAB or because of the small number of correlated positions I used.

I'll implement what is missing to have exactly the same test data to be sure, for my first test I couldn't wait and I simply copied the output values for 4 positions from MATLAB with 6 digits of precision.

It was just a stupid bug in my code, now I get the exact same result in C# and Matlab whatever the number of positions (> 3). I need to test this with real data.

I've updated the previous post with new data.

Nice graphic. It won't be useful for me since I already read the coordinates of the PS Move and the DK2, but that could be useful if you don't want a dependency on Unity for the registration application.

Concerning the registration itself, although the code I've implemented works correctly with test data, it doesn't seem to be appropriate for what we want to solve. From what I understand, what would be needed is an algorithm that solves the AX=YB problem with 3dof sets of data (position only) but I've only seen algorithms intended for 6dof sets of data (position+rotation).

With 6dof data sets the rotation values should be in the same referential than the camera (IMU PS Move -> PS Eye, IMU DK2 -> DK2 camera) but that's not the case, the IMU data for both sets is in the same referential ("real" world, not camera space).

Algorithms that solve the AX = XB problem with 3dof/6dof are also useless for this problem since they don't separate the cam to cam from the PS Move to DK2 transformations.

I know that a AX=YB 3dof algorithm has been implemented (QR15 from Ernst 2012, derived from their QR24 6dof algorithm) but I haven't seen an implementation yet nor do I understand how it does work.

I'm a bit at a loss for now, I'll try to modify the algorithms I've already implemented to handle the 3dof case. If I don't succeed I'll try from scratch by analyzing the problem myself instead of relying on research papers.

I've been thinking about the problem a bit more, trying to understand how it would work in 2D and I'd like some feedback about how I see it.

I think the AX=YB equation is still the one that corresponds to the problem to solve, but I've tried to distribute the values differently.

Considering the 2D case, in the classical algorithm A would be a 3x3 matrix that contains the rotation + translation of the helmet and B would be a 3x3 matrix that contains the rotation + translation of the controller. X and Y would be the unknown 3x3 transforms from one referential to the other one (PS Eye to DK2 camera and PS Move to DK2), which must be solved.

Since the rotations of the helmet and controller are not related to the DK2 camera or PS Eye referentials (they are related to the "world"), I can't use these values inside A and B.

If there was no rotation for the DK2, the transform between the DK2 and the PS Move in the DK2 camera space (the X matrix) would only be a translation. The transformation between the PS Eye and the DK2 camera (the Y matrix) would still be a transformation matrix (rotation + translation).

The matrix A would be only a translation vector (the coordinates of the DK2) and the matrix B would be only a translation vector as well (the coordinates of the PS Move).

But since there is rotation for the DK2, I need to counter it to get the correct coordinates of the PS Move in the DK2 camera referential. So A is now a transform matrix, with the rotation being the inverse of the DK2 rotation, hence the formula presented in this schema :



It seems to work with a simple test case I wrote in Octave (MATLAB syntax) that corresponds roughly to the schema, one case with no DK2 rotation, the second case with a 90° rotation (commented) :

Xx = -1; Xy = -0.5;
%Xx = 0.5; Xy = -1;
X = [Xx Xy 1]';

Ar = 0; Ax = -0.5; Ay = 3.5;
%Ar = -90; Ax = -0.5; Ay = 3.5;
A = [ cos(Ar*pi/180) -sin(Ar*pi/180)   Ax
      sin(Ar*pi/180)  cos(Ar*pi/180)   Ay
      0               0                 1
];

Yr = 45; Yx = 0.5; Yy = 2;

Y = [ cos(Yr*pi/180) -sin(Yr*pi/180)  Yx
      sin(Yr*pi/180)  cos(Yr*pi/180)  Yy
      0       0       1
];

YB = Y * B;
AX = A * X;

In both cases YB and AX are equal, which seems to validate the idea.

I've implemented that in the 3D case with DK2 and PS Move readings but I don't get something that looks like a registration visually. I wonder if it's because I don't enforce the fact that the X rotation should be 0 (ie. identity matrix, the calculated matrix has a rotation close but not equal to the identity matrix) or because what I wrote doesn't make sense.

What do you think about it ?

Hello all.
I tried to get my psMove working a month or 2 ago , Getting the PsMoVE paired and working was no problem but the camera was an issue without the CL-Eye-Driver-5.3.0.0341 driver that I have.
All i ever seemed to get was warnings about missing lens calibration files. :(
But as I had lots of devices installed including having Navi and DS3 controllers working ont op of previously having the CL camera driver installed + I was planning to rebuild my Windows Image and back them up I decided to abandon it until then.

Ive reinstalled my windows and and created my backup images and decided to give this another try before installing anything else other than my core hardware drivers ,Like DS3 controllers and The Cl cam driver.

The PsMove is paired and calibrated and the camera was installed perfectly as instructed from this guide https://github.com/cboulay/psmove-ue4/wiki/Windows-PSEye-Setup
And i noticed i didn't have the ps3eye_sdl.exe mentioned in the bottom of the page so i re-downloaded the latest psmove-ue4 and saw it was included with some other new files and the reg entry file had been removed .
Everything went fine and launching the ps3eye_sdl.exe I can actually see myself and get a picture :) even if its a slight bit more pixlelated than the CL driver version.
But it seems im having the same kind of issue again.

Launching the. tracker_camera_calibration.exe
= Warning: no lens calibration files found.

Launching the. test_tracker.exe
= ### Found 1 controllers.
Trying to init PSMoveTracker...Warning: No lens calibration files found.

I hope this is not a dead end again :(