Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement check for adding constraints to marginalized variables #291

Open
wants to merge 2 commits into
base: devel
Choose a base branch
from
Open

Implement check for adding constraints to marginalized variables #291

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bb77ad7
Implement check for adding constraints to marginalized variables
jakemclaughlin6 Nov 20, 2022
6a0ea4e
Add helper to landmakr variable
jakemclaughlin6 Jan 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 24 additions & 0 deletions fuse_optimizers/src/fixed_lag_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,6 +197,30 @@ void FixedLagSmoother::optimizationLoop()
{
continue;
}
// check if new transaction has added constraints to variables that are to be marginalized
std::vector<fuse_core::UUID> faulty_constraints;
for(auto& c: new_transaction->addedConstraints())
{
for(auto var_uuid: c.variables())
{
for (auto marginal_uuid : marginal_transaction_.removedVariables())
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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. 🤷

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@svwilliams Has this been investigated?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not yet.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

{
if (var_uuid == marginal_uuid)
{
faulty_constraints.push_back(c.uuid());
break;
}
}
}
}
if(faulty_constraints.size() > 0)
{
ROS_WARN_STREAM("Removing invalid constraints.");
for(auto& faulty_constraint: faulty_constraints)
{
new_transaction->removeConstraint(faulty_constraint);
}
}
// Prepare for selecting the marginal variables
preprocessMarginalization(*new_transaction);
// Combine the new transactions with any marginal transaction from the end of the last cycle
Expand Down