Typed Symbols

[contagon]

This PR enables what I'm calling "typed symbols". In a factor graph, each character (ie X or L) will always be of a single type - X is often SE(3) and L is generally in R^3. This feature enforces this constraint at compile time to disallow any type mismatches. This reduces a number of error checking that needs to be done in the Values container and prevents mismatching factor keys from happening.

As part of this, I've introduced a new FactorBuilder class to help facilitate building factors.

When using gtsam it's possible to mis-order the keys when creating a factor. This is now mostly impossible in fact.rs. For example, if proper construction of a stereo factor is,

let factor = FactorBuilder(stereo_factor, X(0), L(0)).build()

then this next line will now fail to compile,

let factor = FactorBuilder(stereo_factor, L(0), X(0)).build()

I see this as being extra helpful once IMU preintegration is added to keep all of it's keys straight.

@taylorpool Currently assigning types is done by the user via a macro, and I could use some help settling on the syntax. I currently have the first one, but have considered all of the following,

assign_symbols!(X: SE3; B: ConstantBias; V: VectorVar3);
assign_symbols!(X => SE3; B => ConstantBias; V => VectorVar3);
assign_symbols!(X = SE3; B = ConstantBias; V = VectorVar3);

Just an FYI, rust also allows all these to be written like,

assign_symbols!{
    X: SE3; 
    B: ConstantBias; 
    V: VectorVar3;
}

I preferred the first one to be similar to python dictionaries, but I'm not quite settled on it yet. We could also switch the semicolons to commas as well. Do you have any thoughts on the syntax? Let me know what you think, I could use an extra opinion on this.

[taylorpool]

This is a really interesting macro... Not a blocker, but if there is any way to not do this using macros, that might be better

[contagon]

You can do it without macros, it's just a good amount of copying and pasting. I also prefer the macros, as if we decide to support factors with > 6 variables, it's a one-liner to add in support.

I would've preferred a single new function as well instead of new1, new2, etc, but couldn't figure out a way to do it, mainly since each of our keys might have a different type now

[taylorpool]

Any way to not make this dynamic? i.e. do it at compile time

[contagon]

We could probably make it done at compile time, but I'm not sure if there's a huge advantage? As it'll end up boxing them to send to Factor when building anyways.

At one point I tried to not box anything, but it got hairy since that mention Factor was generic over the residual, noise, and robust kernel. It made storing them all in Graph nigh-impossible

[taylorpool]

Slick way to check at compile time that the dimension of the noise is correct...I wish GTSAM did this. Nice job!

[contagon]

Yeah, I was pretty happy with that one :) one of my least favorite gtsam experiences it hitting a weird branch in the code and crashing due to noise model sizes

If you have any other gtsam issues you think could be solved at compile time, let me know and we can investigate

[taylorpool]

If it doesn't need to be used by hand, maybe consider making this not public?

[contagon]

That'd probably be a good idea. Currently pretty much everything is public, and there's a lot of the low level interface that shouldn't be. I haven't taken the time to think about it thoroughly.

[contagon]

@taylorpool Did you have anything you'd like to see changed on this? Or am I good to merge?