Calibration math

[Doc-Ok]

It turns out the calibration math is quite simple. If you represent a tracking result from either system as a 4x4 matrix, where the upper-left 3x3 matrix is a rotation matrix, the upper-right 3x1 matrix is a translation vector (or position), and the lower 1x4 matrix is (0, 0, 0, 1), then you can write the calibration system as

for all i: Ai * L = G * Bi

where Ai, Bi are a tracking sample pair, L is the local transformation between the two devices, and G is the calibration matrix you're looking for, which takes a tracking sample from system B and transforms it to system A. All matrices are 4x4.

If you multiply that formula out for a single tracking sample pair, you get 12 linear equations in 24 unknowns. If you put a bunch of observations into a standard linear least-squares system, you can solve for matrices L and G directly (and then ignore L). Approach: each tracking pair adds 12 rows to matrix A (which has 24 columns), and one row to the right-hand-side vector b. After n pairs, matrix A is n12 x 24, and b is n12 x 1. You then solve the normal system A^T * A * x = A^T * b using a pivoting Gaussian solver, which yields the 24 unknown coefficients of L and G. (For efficiency, don't construct A or b but construct A^T * A, which is 24x24, and A^T * b, which is 24x1, on-the-fly, one tracking sample pair at a time.)

The upper-left 3x3 matrix of G will be close to a scaled rotation matrix (with scaling close to 1 if the two tracking systems use the same linear unit), and you can clean it up via polar decomposition.

[Doc-Ok]

Just in case, here's actual code. It uses a matrix class and a transformation class, but should be obvious:

State:

Math::Matrix ata(24,24,0.0); // ATA, 24x24, all zeros
Math::Matrix atb(24,1,0.0); // ATb, 24x1, all zeros

Per tracking sample pair:

/* Write sample from system A into 4x4 matrix: */
Geometry::Matrix<double,4,4> m0(1.0); //Initialized to identity
devs[0]->getTransformation().writeMatrix(m0); // Writes 3x4, leaves bottom row alone

/* Write sample from system B into 4x4 matrix: */
Geometry::Matrix<double,4,4> m1(1.0); //Initialized to identity
devs[1]->getTransformation().writeMatrix(m1); // Writes 3x4, leaves bottom row alone

/* Generate 12 equations in three blocks of 4 equations each: */
double eq[24]; // Temporary coefficient storage
for(int b=0;b<3;++b)
	{
	/* Four equations per block: */
	for(int e=0;e<4;++e)
		{
		/* Reset the equation: */
		for(int i=0;i<24;++i)
			eq[i]=0.0;
		
		/* Fill in the current equation: */
		eq[b*4+0]=m0(0,e);
		eq[b*4+1]=m0(1,e);
		eq[b*4+2]=m0(2,e);
		eq[b*4+3]=m0(3,e);
		
		eq[12+e]=-m1(b,0);
		eq[16+e]=-m1(b,1);
		eq[20+e]=-m1(b,2);
		
		double rhs=e==3?m1(b,3):0.0;
		
		/* Enter the equation into the least-squares system: */
		for(int i=0;i<24;++i)
			{
			for(int j=0;j<24;++j)
				ata(i,j)+=eq[i]*eq[j];
			atb(i)+=rhs*eq[i];
			}
		}
	}

After all samples have been entered:

/* Solve the least-squares linear system: */
Math::Matrix x=atb.divideFullPivot(ata); // Gauss elimination with full pivoting

/* Assemble the affine pre- and post-transformation matrices: */
Geometry::AffineTransformation<double,3> preA,postA; // Two 3x4 matrices
for(int i=0;i<3;++i)
	for(int j=0;j<4;++j)
		{
		preA.getMatrix()(i,j)=x(0+i*4+j);
		postA.getMatrix()(i,j)=x(12+i*4+j);
		}

[pushrax]

Thanks for the comment! This idea simplifies the approach a lot.

Since the initial implementation, I figured out why I was having trouble trying to get a single-step solution to work: the origin of rotation for G was not always zero – matzman666/OpenVR-InputEmulator#113. With the new driver in 1bed4af, this problem should be fixed.

[PowerfulPony]

@pushrax Hi!

Why knowing 3D models of controllers, do not calibrate their location in space, just putting the controllers next to each other in a predetermined position?

[fangguanya]

@Doc-Ok Hi,professor, thanks for your sharing, this is really helpful for me ,and I cost nearly half a day to understanding your idea.
After some calculation, I have a question.

In my opinion, because of the existance of vector (0,0,0,1) at both side of the equation : AL=GB, There must be a single element at both side, with the calculation, it is : a14, G14, but I cannot setup a relevance between your idea with my calculation.

I guess I did some stupid mistake, and there is no one can help me at my side ..
Could you please take a look, and give me some advice?

Really thanks, professor.

[fangguanya]

@Doc-Ok Finally, I got it....thanks, I negate the wrong side..

[pushrax]

I implemented a single system solver as described here in 9040063, but at the time could not get it to match the reliability of the previous algorithm. I tried a variant with polar decomposition as well. The single system seemed to be more sensitive to error induced by sample timing mismatch between the references devices.

I have a feeling the reliance on only rotation data to calibrate rotation in the old algorithm could explain its better performance. Under movement, this would be dependent much more on gyro data and reject accelerometer data.

I'm going to close this issue now as the current system is performing well and I don't have much of a reason to continue trying to understand/fix the gap in performance. Thanks very much for the information though! It's been a good exercise.