Make Values.TangentVector == VectorValues

[marcrasi]

This sets Values.TangentVector to VectorValues, which allows a few nice improvements to the APIs:

• To update a Value using a VectorValues, now all you need is vals.move(along: vectorval) instead of needing to loop over all the components and cast them.
• We can now write a generic JacobianFactor initializer that linearizes any error function, which simplifies the implementation of linearize.

Other API changes:

• Now you access values using vals[key, as: Pose2.self] instead of vals[key].baseAs(Pose2.self). This is necessary so that Values knows what type you are accessing and therefore knows how to convert its tangent vector to a Vector suitable for inclusion in VectorValues.
• Now you insert values with vals.insert(key, value) instead of vals.insert(key, AnyDifferentiable(value). (It still gets stored as a type-erased AnyDifferentiable though).

[ProfFan]

Nice! However I think I am missing some pieces in merging the previous PR into the generic NLFG in #44, specifically, differentiability on VectorConvertible, as we need to take the Jacobian of a VectorConvertible conforming object in error(). Could you try merging feature/nonlinear_factor_graph and see what is the mileage for you?

[marcrasi]

Nice! However I think I am missing some pieces in merging the previous PR into the generic NLFG in #44, specifically, differentiability on VectorConvertible, as we need to take the Jacobian of a VectorConvertible conforming object in error(). Could you try merging feature/nonlinear_factor_graph and see what is the mileage for you?

My quick guess is that all you need to do is add this where clause to the factor declarations:

public struct BetweenFactor<T: LieGroup>: NonlinearFactor where T.TangentVector: VectorConvertible

I don't have time to actually try it out in code today, so if you haven't gotten it yet, I'll try it out tomorrow and figure out if it actually works.

[ProfFan]

@marcrasi I pushed the WIP on #44, the error being:

/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:35: error: expression is not differentiable
    return error.vector.squared().sum()
                                  ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:35: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    return error.vector.squared().sum()
                                  ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:25: error: expression is not differentiable
    return error.vector.squared().sum()
                        ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:25: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    return error.vector.squared().sum()
                        ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:18: error: expression is not differentiable
    return error.vector.squared().sum()
                 ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:18: note: member is not differentiable because the corresponding protocol requirement is not '@differentiable'
    return error.vector.squared().sum()
                 ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:35: error: expression is not differentiable
    return error.vector.squared().sum()
                                  ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:35: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    return error.vector.squared().sum()
                                  ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:25: error: expression is not differentiable
    return error.vector.squared().sum()
                        ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:25: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    return error.vector.squared().sum()
                        ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:18: error: expression is not differentiable
    return error.vector.squared().sum()
                 ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/BetweenFactor.swift:73:18: note: member is not differentiable because the corresponding protocol requirement is not '@differentiable'
    return error.vector.squared().sum()
                 ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:43: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                                          ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:43: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    let residual = error.vector.squared().sum()
                                          ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:33: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                                ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:33: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    let residual = error.vector.squared().sum()
                                ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:26: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                         ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:26: note: member is not differentiable because the corresponding protocol requirement is not '@differentiable'
    let residual = error.vector.squared().sum()
                         ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:43: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                                          ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:43: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    let residual = error.vector.squared().sum()
                                          ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:33: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                                ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:33: note: cannot differentiate functions that have not been marked '@differentiable' and that are defined in other files
    let residual = error.vector.squared().sum()
                                ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:26: error: expression is not differentiable
    let residual = error.vector.squared().sum()
                         ^
/Users/proffan/Projects/Development/CV/SLAM/SwiftFusion/SwiftFusion/Sources/SwiftFusion/Inference/PriorFactor.swift:45:26: note: member is not differentiable because the corresponding protocol requirement is not '@differentiable'
    let residual = error.vector.squared().sum()

I tried some tweaking on the generated code to add @differentiable but didn't see success.

[marcrasi]

Thanks, I'm starting to look now!

[marcrasi]

I'll rebase this on feature/nlfg_with_manifold so that you do not have to do that. I'll open a new PR once that is ready.

[marcrasi]

Ok, the rebased version is #50.