Implement check for adding constraints to marginalized variables

[jakemclaughlin6]

This PR adds a check for constraints that are added to variables that are about to be marginalized. Addressing this issue: #248

[jakemclaughlin6]

@svwilliams Can you review this?

[svwilliams]

Ah, I understand now. I'm playing some games with the sequence of events in order to minimize publish latency. The correct order of operations should be:

• Add new transactions to the graph
• Optimize the graph
• Compute marginal transaction
• Apply marginal transaction to the graph
• Publish results (notify)

But the "compute marginal transaction" is non-trivial computationally, so I reordered the sequence:

• Gather new transactions
• Gather previous marginal transaction
• Update the graph with both sets of changes
• Optimize the graph
• Publish results (notify)
• Compute marginal transaction

What I think is causing problems is that the marginal transaction and new sensor transactions are being applied at the same time. If the sensor transaction involves the to-be-marginalized variable, then it is both used and removed at the same time.

A better sequence would have been:

• Apply previous marginal transaction to the graph
• Add new transactions to the graph
• Optimize the graph
• Publish results (notify)
• Compute marginal transaction

By applying the previous marginal transaction to the graph before applying the new sensor transactions, then the variable would be removed in the first step, then added back to the system in the second step. Which is what would have happened if I had implemented the correct operation order.

An issue that still remains in the revised order is that a variable that will be marginalized out is not communicated to the rest of the system. The sensor and motion models have no way of knowing that a variable is about to be marginalized out, and thus may think that using that variable in a future transaction is still perfectly fine.

Let me do some performance profiling on exactly how much time is spent in the "compute marginal" step. If it is not as slow as I remember, implementing the correct operation order would be a more principled solution to this issue.

An issue I have with the proposed solution is that a Transaction was intended to be an atomic unit of work. Either all of the operations are applied, or none of them are. My reasoning for this are systems like monocular visual odometry that require special care to initialize the landmarks. If part of the landmark initialization cannot be applied, then none of the landmark observations should be applied either. The proposed changes here are splitting up a transaction into individual constraints, which violates the atomic assumptions. We could throw away the entire transaction instead if it includes references to deleted variables. 🤷

[jakemclaughlin6]

@svwilliams Has this been investigated?

[svwilliams]

Not yet.

[jakemclaughlin6]

I have implemented and tested the first proposed proper order and the marginalization step stays under 1ms in almost every case, I can try to gather some data comparing marginal transaction size (# vars, # constraints) versus marginalization runtime if that would be helpful in your decision

[jakemclaughlin6]

@svwilliams I was curious if this has been investigated?