search

borglab / SwiftFusion

Apache License 2.0
115 stars 13 forks source link

Subscripted View for Fixed-Sized Matrices and Vectors #105

Open ProfFan opened 4 years ago

ProfFan commented 4 years ago

When implementing the chordal initialization I found out that it is almost essential that we have:

  1. a mutable, subscripted view into the fixed-size matrices;
  2. methods like MatrixN.zero and MatrixN.identity.

Otherwise I end up writing code like this:

let M9: Jacobian9x9_2 = Array9([
        Tuple2(Vector9(-1,0,0,0,0,0,0,0,0), Vector9(Rij.s00,Rij.s01,Rij.s02,0,0,0,0,0,0)),
        Tuple2(Vector9(0,-1,0,0,0,0,0,0,0), Vector9(Rij.s10,Rij.s11,Rij.s12,0,0,0,0,0,0)),
        Tuple2(Vector9(0,0,-1,0,0,0,0,0,0), Vector9(Rij.s20,Rij.s21,Rij.s22,0,0,0,0,0,0)),
        Tuple2(Vector9(0,0,0,-1,0,0,0,0,0), Vector9(0,0,0,Rij.s00,Rij.s01,Rij.s02,0,0,0)),
        Tuple2(Vector9(0,0,0,0,-1,0,0,0,0), Vector9(0,0,0,Rij.s10,Rij.s11,Rij.s12,0,0,0)),
        Tuple2(Vector9(0,0,0,0,0,-1,0,0,0), Vector9(0,0,0,Rij.s20,Rij.s21,Rij.s22,0,0,0)),
        Tuple2(Vector9(0,0,0,0,0,0,-1,0,0), Vector9(0,0,0,0,0,0,Rij.s00,Rij.s01,Rij.s02)),
        Tuple2(Vector9(0,0,0,0,0,0,0,-1,0), Vector9(0,0,0,0,0,0,Rij.s10,Rij.s11,Rij.s12)),
        Tuple2(Vector9(0,0,0,0,0,0,0,0,-1), Vector9(0,0,0,0,0,0,Rij.s20,Rij.s21,Rij.s22))
      ])

Moreover, it becomes almost impossible to write code when I try to implement SVD for 3x3 rotation matrices as I cannot write stuff like A[0, 0], and I cannot dynamically index the matrix in for loops...

I am not sure if this design is good in a "Swifty" sense, so please correct me if I make silly mistakes!

marcrasi commented 4 years ago

These operations sound good to me!

We've talked a lot about the ideal fixed size matrix and I think we still haven't figured it out yet, but that shouldn't stop us from having a quick and dirty implementation that supports all the operations you need. I think I could have one ready in a few hours.