Conversion between FixedSizeMatrix and Jacobian

[ProfFan]

Consider the following test:

func testPose3SimplePriorFactor() {
    let P = Vector3(3.5,-8.2,4.2)
    let R = Rot3.fromTangent(Vector3(0.3, 0, 0))
    let t1 = Pose3(R, P)
    let I = FixedSizeMatrix<Array6<Vector6>>.identity
    
    var val = VariableAssignments()
    let id1 = val.store(t1) // should be identity matrix
    let prior_factor = PriorFactor3(id1, t1)
    // Change this to t2, still zero in upper left block
    
    let actual = prior_factor.linearized(at: Tuple1(t1))
    
    print(actual.jacobian)
    //assertAllKeyPathEqual(actual.jacobian, I, accuracy: 1e-8)
  }

actual.jacobian is Array6<Tuple1<Vector6> and I is Array6<Vector6>, so the assertion cannot compile.

@marcrasi Any thoughts? Thanks in advance :)

[ProfFan]

Currently I am doing this (toxic) trick:

  func testPose3SimplePriorFactor() {
    let P = Vector3(3.5,-8.2,4.2)
    let R = Rot3.fromTangent(Vector3(0.3, 0, 0))
    let t1 = Pose3(R, P)
    let I = FixedSizeMatrix<Array6<Vector6>>.identity
    
    var val = VariableAssignments()
    let id1 = val.store(t1) // should be identity matrix
    let prior_factor = PriorFactor3(id1, t1)
    // Change this to t2, still zero in upper left block
    
    let actual = prior_factor.linearized(at: Tuple1(t1))
    
    var jacobian = actual.jacobian

    let voidPtr = withUnsafeMutablePointer(to: &jacobian) {
        $0.withMemoryRebound(to: FixedSizeMatrix<Array6<Vector6>>.self, capacity: 1) {
            (unsafePointer: UnsafeMutablePointer<FixedSizeMatrix<Array6<Vector6>>>) in

            unsafePointer
        }
    }

    // print(voidPtr.pointee)
    XCTAssertEqual(voidPtr.pointee, I)
  }

[marcrasi]

One thing you could do is let I = FixedSizeMatrix<Array6<Tuple1<Vector6>>>.identity, which makes it have the same type as the jacobian.

[marcrasi]

There's not a similar trick for constructing an .identity that works with JacobianFactors with multiple jacobians (e.g. Tuple2, Tuple3, ...). If you need to do that, the best I can think of would be to construct the whole expected jacobian explicitly:

let expected = FixedSizeMatrix<Array6<Tuple2<Vector6, Vector6>>>([
  ...
])

The APIs for dealing with stuff like clearly need improvement. Maybe it's possible to make the public field of the JacobianFactor be a tuple of FixedSizeMatrix so that it's easier to access the individual jacobians. I'll think about this and I might have a PR for it tomorrow.

[ProfFan]

One thing you could do is let I = FixedSizeMatrix<Array6<Tuple1<Vector6>>>.identity, which makes it have the same type as the jacobian.

This works great :)

There's not a similar trick for constructing an .identity that works with JacobianFactors with multiple jacobians (e.g. Tuple2, Tuple3, ...). If you need to do that, the best I can think of would be to construct the whole expected jacobian explicitly:

let expected = FixedSizeMatrix<Array6<Tuple2<Vector6, Vector6>>>([
  ...
])

The APIs for dealing with stuff like clearly need improvement. Maybe it's possible to make the public field of the JacobianFactor be a tuple of FixedSizeMatrix so that it's easier to access the individual jacobians. I'll think about this and I might have a PR for it tomorrow.

Yeah I think something like .jacobians[0] would be great!