Compute PseudoInverse from 4x3 Matrix

Projects Programming
1

Hi

I think there is no existing library which allows me to compute the pseudoinverse from a 4x3 matrix (4 rows, 3 collums). At least I found this: http://www.rejonesconsulting.com/indexlibrary.htm
The matrix.h file should allow me to compute my matrix. but unfortunately i can't just include this file an work with all of the matrix functions. So my question is:

  1. Is there an existing library for computing the pseudo inverse for a 4x3 Matrix?

  2. Can i include the matrix.h file in general and am I just doing it wrong?

  3. Do i need to refactor the whole header file (~4k rows code) and create several classes and headers like basically described here: http://www.arduino.cc/en/Hacking/LibraryTutorial?

greetings

(I just want the pseudo inverse... But this suddenly seems like an awful amount of work)

2

If you are just doing this once, post the matrix. One of us will use a more suitable program (e.g. Matlab) to calculate it. Takes one line of code.

3

Is this matrix used to store rotation, scale and translation?

If its only used for translation and rotation, then an "easy" inverse is the transpose of the 3x3 rotational part, and the inverse translation part is the negative of the dot products of the translation and corresponding 3x3 rotaiton axis

4

@jremington: I could do that in wolfram alpha for example. But for calibration purposes i need to do it on the arduino

@tammytam: I'm not sure. The Matrix i need to invert looks like this:
A[0][0] = 1;
A[1][0] = 0;
A[2][0] = 1;
A[3][0] = 0;
A[0][1] = 0;
A[1][1] = 1;
A[2][1] = 0;
A[3][1] = 1;
A[0][2] = -distanceFromZeroSpeedTrig.sin(0);
A[1][2] = distanceFromZeroSpeedTrig.cos(0);
A[2][2] = -distanceFromZeroSpeedTrig.sin(180);
A[3][2] = distanceFromZeroSpeedTrig.cos(180);

It's described here: http://www.researchgate.net/publication/221644902_Precise_dead-reckoning_for_mobile_robots_using_multiple_optical_mouse_sensorsx4
Page: 147 (left side, center)

5

Sorry Dungeon, I don't have time to look at that link tonight. But assuming you definitely don't have scale in your matrix (else you'll need to use Gaussian elimination), you can use the following method: (Although from your matrix, it looks as though it is normalised, also unsure as whether this will work with non orthogonal matricies (I've never tried !))

Transpose your 3x3 section, whcih means mirror across the diagonal.

Take the translation part (dependant on whether you use column major / row major, its usually the last row or last column), and dot product it with the relevant axis, then negate it:

m[4][3] // matrix definition
inv_m[4][3] // inverse

// transpose
inv_m[0][1] = m[1][0]
inv_m[0][2] = m[2][0]
inv_m[1][1] = m[1][1]
inv_m[1][2] = m[2][1]
inv_m[2][1] = m[1][2]
inv_m[2][2] = m[2][2]

// Putting dot product working out here and you can apply it below

float dot = ( vec1.x * vec2.x ) + ( vec1.y * vec2.y ) + ( vec1.z * vec2.z ) );

// translation part (A matrix row is a xyz vector)
inv_m[3][0] = -dot_product(m[3], m[0]);// trans dotted with x axis
inv_m[3][1] = -dot_product(m[3], m[1]);// trans dotted with y axis
inv_m[3][2] = -dot_product(m[3], m[2]);// trans dotted with z axis

Clear as mud? Bit of a mess but should hopefully make sense, from that you've constructed a new inverse matrix from a matrix.

6

:smiley:
Thanks. I'm going to look into that tomorrow. Maybe then it will make sense to me^^

7

Sorry Dungeon, I'm being a bit dense, although you mentioned it several times it still didn't occur to me you were attempting to work out the pseudo inverse.

The method I described won't help in that regard, I've never tried to calculate one from scratch before, and to be honest, I probably couldn't as the math is beyond me. All I know is it is a brute force recursive method that's usually used, and as jremington posted, usually a job for matlab/wolfram etc.

Found this link that might help you out though, didn't look too taxing:

http://sourceware.org/ml/gsl-discuss/2008-q2/msg00013.html

8

No problem! I'm glad someone helps me :wink: So thank you anyway!

Ok. Then lets make this a thing about programming (considering that pseudo inverse matrices are beyond my math skills atm). As I said above I have got this matrix.h library. (http://www.rejonesconsulting.com/indexlibrary.htm). But would it be possible to use this library, either as it is, or split it into several h and cpp files?

Then I wouldn't need to worry about the actual matrix stuff.

9

Looks less like a library and more like a header file full of code hehe.

Don't see why it wouldn't work. However if I was doing it, I'd firstly chop out ALL but the bits I needed, less bloat on the limited space an Arduino offers. I would literally cut out everything bar the pseudo inverse function and any functions it depends on.

I would then strip out all the iostream nonsense, thats all cout, cin cerr etc.

Might run like an absolute dog though, have a look through all those functions required to work out the pinv, in particular, the svd() function. There is enough nested divisions and square roots on doubles to make a modern Intel choke :wink:

10

Yeah I can see what you mean. I'm sure it's pretty efficient though ^^

Ok. Atm I don't care about memory anyway (I will do that later). I'm just trying to import the matrix.h file like:

#include <matrix.h>
Matrix m;

setup(){
m = new Matrix();
}

Which says: Matrix does not name a type
and: m was not declared in this scope (ok this one is related to Matrix not being a type)

So what am I doing wrong? There is a class Matrix and an empty constructor inside of matrix.h

EDIT:
If I do:
#include "matrix.h"

I get a lot lf errors regarding the matrix.h file itself:
In file included from ADNS9500.ino:9:
matrix.h:4015: error: stray '\32' in program
/matrix.h: In function 'void prompt()':
matrix.h:130: error: 'cout' was not declared in this scope
matrix.h:130: error: 'cin' was not declared in this scope
/matrix.h: In function 'void separate()':
matrix.h:131: error: 'cout' was not declared in this scope
matrix.h:131: error: 'cout' was not declared in this scope
matrix.h:131: error: 'endl' was not declared in this scope
/matrix.h: In function 'void separateX()':
matrix.h:132: error: 'cout' was not declared in this scope
....
matrix.h:802: error: 'Matrix' does not name a type
....

Maybe this is the right way to import the file but, these functions are simply missing in arduino in general. However the Matrix not being a type problem persists.

11

If your matrix A is a 4x3 matrix, then your pseudoinverse is a 3x4 matrix B which satisfies

ABA=A

where your first product AB is going to be 4x4 and your second product (AB)xA is going to be 4x3 again, and equal to A.

In your case, m>n ( 4>3 ), so one method to calculate the pseudoinverse is to calculate B =(inv(A* x A)) x A

where A* is the transpose of A ( a 3x4 matrix ), the product A* x A is 3x3, its inverse is 3x3 and the result B is then supposedly the product of a 3x3 and a 4x3 matrix, which is invalid. I don't know how they expect that algorithm to work.

But anyhow, supposing that algorithm is right and I have misinterpreted it somehow, it is quite easy to write your own code to calculate those steps, for small matrices.

12

OK, I have found the problem in my previous post.

In your case, m>n ( 4>3 ), so one method to calculate the pseudoinverse is to calculate

B =(inv(A* x A)) x A*

where A* is the transpose of A ( a 3x4 matrix ), the product A* x A is 3x3, its inverse is 3x3 and the result B is then supposedly the product of a 3x3 and a 3x4 matrix, which is 3x4

This matrix will meet the criterion B x A = I3 where I3 is the 3x3 identity matrix.

For this to work, there are some conditions on the independence of the rows of A.

13

#include <matrix.h>
Matrix m;

setup(){
m = new Matrix();
}

Which says: Matrix does not name a type
and: m was not declared in this scope (ok this one is related to Matrix not being a type)

So what am I doing wrong? There is a class Matrix and an empty constructor inside of matrix.h

There may be an empty constructor for class Matrix, but it is probably a useless one. Any sensible matrix class is going to require you to name the dimensions in its constructor.

Also, you are using the new keyword, which implies an object allocated in dynamic memory, which is fine but probably required that the variable m be of type "pointer to matrix"

14

Ok now I doing this:

  A[0][0] = 1;
  A[1][0] = 0;
  A[2][0] = 1;
  A[3][0] = 0;
  A[0][1] = 0;
  A[1][1] = 1;
  A[2][1] = 0;
  A[3][1] = 1;
  A[0][2] = distanceFromZero*SpeedTrig.sin(0);
  A[1][2] = -distanceFromZero*SpeedTrig.cos(0);
  A[2][2] = distanceFromZero*SpeedTrig.sin(180);
  A[3][2] = -distanceFromZero*SpeedTrig.cos(180);
  
  // Computing the following:
  // B =(inv(A* x A)) x A*
  Matrix.Transpose((float*)A, 4, 3, (float*)At);
  Matrix.Multiply((float*)A, (float*)At, 3, 4, 3, (float*)AAtP);
  Matrix.Invert((float*)AAtP, 3);
  Matrix.Multiply((float*)AAtP, (float*)At, 3, 3, 4, (float*)pseudoInverseA);

This results in this Matrix:

0.52 0.04 0.52 -0.03
5.29 0.48 5.29 1.44
0.48 -0.04 0.48 0.03

Where wolfram alpha gives me:
pseudoinverse{{1, 0, 0},{0, 1, -12.9921},{1, 0, 0},{0, 1, 12.9921} - Wolfram|Alpha}

0.5 0.0 0.5 0.0
0.0 0.5 0.0 0.5
0.0 -0.0384849 0.0 0.0384849

To me it seems like a lot of rounding problems, plus: [0,1], [2,1], [1,3], [0,2] and [2,2] are just wrong.

I will look into the c++ coding stuff as well. Thanks.

15

The sin() and cos() functions take arguments in radians, not degrees.

16

Just throwing this out there, but watch out for column major vs row major, and other weird crap like handiness of space (though that shouldn't effect you on just an inverse).

17

SpeedTrig.cos(180) = -1
SpeedTrig.sin(180) = 0

this seems right.

18

Dungeon:
SpeedTrig.cos(180) = -1
SpeedTrig.sin(180) = 0

this seems right.

How did you arrive at those values?

19 
Serial.print("Cos180 is: ");
Serial.println(SpeedTrig.cos(180));
20

Ah Dungeon, your using speedTrig. Pretty certain it uses lookup tables to speed up the trig functions, so your bount to get a much lower resolution on your results, try swapping out to built in trig functions, and like Paul said, may need to switch to rads.

Though they shouldn't clip that badly.