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.

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

Add node marginalization support #468

Open
wants to merge 6 commits into
base: noetic-devel
Choose a base branch
from

Conversation

hidmic
Copy link
Collaborator

@hidmic hidmic commented Jan 14, 2022

Basic Info

Info Please fill out this column
Ticket(s) this addresses Second half of #407
Primary OS tested on Ubuntu
Robotic platform tested on CPR's Jackal gazebo simulation [*]

[*] See gist for instructions on how to replicate it.


Description of contribution in a few bullet points

  • Exposed Ceres solver information matrix for covariance estimation of marginalized distributions.
  • Added node marginalization support, based on a Chow-Liu tree approximation to Karto's graph.
  • Modified experimental lifelong mapping node to optionally use node marginalization (instead of plain removal).

Description of documentation updates required from your changes

  • Added new parameter, so I probably need to add that to the documentation page.

Future work that may be required in bullet points

  • This patch essentially reverts Reverted solver blocks removal that crashes multiple initial poses #445. I haven't run into issues myself, but I'm not entirely sure I follow what the failure mode was.
  • Testing, lots of testing. This has been manually tested on a simulation only. It must be validated on a real mobile robot.
  • Port to ROS 2. The only reason this patch targets ROS 1 Noetic is third-party simulation availability.

@hidmic hidmic changed the title Add node marginalization support. Add node marginalization support Jan 14, 2022
@SteveMacenski
Copy link
Owner

SteveMacenski commented Jan 14, 2022

This is frankly so much code, I cannot reasonably review it. I'm not really sure what I can promise to be done here. There's enough going on and I don't have the time to do the background research to understand what you've done to critically review this. Is there someone that could take a look at this that would better understand and be able to provide technique and software reviews?

I've long since moved on from 2D SLAM into navigation world so this is also unfamiliar to me at this moment.

@hidmic
Copy link
Collaborator Author

hidmic commented Jan 14, 2022

This is frankly so much code, I cannot reasonably review it. I'm not really sure what I can promise to be done here.

Assuming that LOC count is the problem here, I think it would be possible to split this. At least to some extent.

Is there someone that could take a look at this that would better understand and be able to provide technique and software reviews?

Hmm, good question. Perhaps I can ask folks over at Nav2's Slack? Or anybody at the Nav2 WG?

I've long since moved on from 2D SLAM into navigation world so this is also unfamiliar to me at this moment.

That's fair. It is a bit intricate, that's true.

@SteveMacenski
Copy link
Owner

It's not just LOC but also what's happening in those LOC. Though the length certainly doesn't help 😆

No one that I can think of, there's not many people familiar with this codebase or the intricacies of SLAM, unfortunately.

@hidmic
Copy link
Collaborator Author

hidmic commented Jan 30, 2022

@SteveMacenski I've reached out to @Shokman to help review this contribution. He's a former co-worker and a friend with expertise in the area and no ties to Ekumen Inc. as of today. He has kindly accepted to do so.

If you're onboard, I'll close this PR, split it in 2 or 3 smaller PRs, and kick start the review.

slam_toolbox/lib/karto_sdk/src/Mapper.cpp Outdated Show resolved Hide resolved
@@ -37,7 +38,9 @@ CeresSolver::CeresSolver() :
first_node_ = nodes_->end();

// formulate problem
angle_local_parameterization_ = AngleLocalParameterization::Create();
local_parameterization_ = new ceres::ProductParameterization(
Copy link
Owner

Choose a reason for hiding this comment

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

Please explain this

Copy link
Collaborator Author

@hidmic hidmic Mar 18, 2022

Choose a reason for hiding this comment

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

I changed the formulation of the NLS problem to optimize SE3 variables instead of 3 scalar variables so as to maintain a 1-to-1 relationship between nodes and parameter blocks. Error terms are equivalent, but this way I can reconstruct the block ordering to make sense of the information matrix.

Copy link
Owner

Choose a reason for hiding this comment

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Hmm, I had not seen that. It looks like Parameterization abstractions have been deprecated in Ceres 2.1.0rc1, but as far as I can see that version is not yet available in any Linux distro. Certainly not for Debian/Ubuntu, and by the looks of it I don't think it'll make it to Jammy.

We should probably keep an eye on it though, we may have to transition in the next couple years.

std::vector<double*> parameter_blocks;
problem_->GetParameterBlocks(&parameter_blocks);
for (auto * block : parameter_blocks) {
(*ordering)[(*nodes_inverted_)[block]] = index;
Copy link
Owner

Choose a reason for hiding this comment

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

Please explain this

Copy link
Collaborator Author

@hidmic hidmic Mar 18, 2022

Choose a reason for hiding this comment

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

I need a mechanism to recover the order of each node variable to make sense of the information matrix. When computing gradients and jacobians, Ceres matches the parameter block order (see here). Parameter block order matches insertion order, until you remove a block. Then that ordering is lost. Completely. This bit of code reconstructs that ordering, using a hashmap to recover the node ID for a given block.

Copy link
Owner

Choose a reason for hiding this comment

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

Is it not possible that we can combine the 2 maps with a struct so we have 1 data structure containing all of this information? I don't love the idea of having these 2 independent tables, it doesn't seem good

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Hmm, what do you mean exactly? To build a bi-directional map of sorts backed by two hash maps? Or to have an actual bi-directional map?

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Well, using a proper bidirectional map was a tiny bit more troublesome than anticipated, as what I was doing here wasn't quite it (i.e. Eigen::Vector3d isn't double *). See 1041a52. On the flip side, using a pool for parameter blocks may turn out to be beneficial to cache performance.

Copy link
Owner

Choose a reason for hiding this comment

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

Why not create a struct as the value in the map that contains more information than just the single entry?

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Why not create a struct as the value in the map that contains more information than just the single entry?

Hmm, I don't follow. The original hash map would map node ID to parameter block. I need to perform the reverse query. My previous approach used two hash maps. The current one uses a bidirectional map. Are you proposing to have a single hash map and perform a linear search every time we need to map a parameter block to a node ID? That's O(n (n + 1) / 2) in the worst case (i.e. next parameter block always found last).

@@ -37,7 +38,9 @@ CeresSolver::CeresSolver() :
first_node_ = nodes_->end();

// formulate problem
angle_local_parameterization_ = AngleLocalParameterization::Create();
local_parameterization_ = new ceres::ProductParameterization(
Copy link
Owner

Choose a reason for hiding this comment

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

This is newed but never deleted in reset / destructor

Copy link
Collaborator Author

@hidmic hidmic Mar 25, 2022

Choose a reason for hiding this comment

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

Ahh, good catch. Fixed in aa9ffa6.

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

BTW what's the point of dynamically allocating most CeresSolver member fields? Barring those that must be because Ceres' API requires it, it doesn't seem to add much value -- it only opens the door to leakage if constructor, destructor, and reset implementations ever end up out of sync.

Copy link
Owner

Choose a reason for hiding this comment

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

I think the things that are dynamically allocated are thoes that Ceres requires.

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Hmm, those std::unordered_map members are not required by Ceres.

Copy link
Owner

Choose a reason for hiding this comment

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

I don't understand what you're referencing this comment is on AngleLocalParameterization which is a ceres-owned object . I personally don't like that I had it raw pointers and the expectation is that it deals with the lifecycle, but that's what Ceres docs said when I was looking at building this

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

I don't understand what you're referencing this comment is on AngleLocalParameterization

Yes, you're right, my bad. I was referring to e.g.

std::unordered_map<int, Eigen::Vector3d> * nodes_;
std::unordered_map<size_t, ceres::ResidualBlockId> * blocks_;

Anyhow, it's orthogonal to this patch.

sqrt_information(2,2) = precisionMatrix(2,2);

Eigen::Matrix3d sqrt_information =
pLinkInfo->GetCovariance().Inverse().ToEigen().llt().matrixL();
Copy link
Owner

Choose a reason for hiding this comment

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

Did we want to use the new one from #473 ?

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Yes! But I have to backport it to Noetic first.

Copy link
Owner

Choose a reason for hiding this comment

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

Isbackported() {return true;};

Copy link
Collaborator Author

@hidmic hidmic Apr 4, 2022

Choose a reason for hiding this comment

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

Indeed. I've rebased this PR since.

edge->GetSource()->GetObject()->GetUniqueId(),
edge->GetTarget()->GetObject()->GetUniqueId());
m_pGraph->RemoveEdge(edge);
delete edge;
Copy link
Owner

Choose a reason for hiding this comment

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

Need to set to null so that any future check for if(ptr) will return false, that will not be done if not set to null ptr

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Hmm, there are no future checks, and edge is scoped to the loop. Do you intend to set it to nullptr just to play it safe?

Copy link
Owner

@SteveMacenski SteveMacenski Mar 25, 2022

Choose a reason for hiding this comment

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

Yeah, and there where places where this bit me in the ass badly when I was adding graph object removal and serialization. Anything deleted manually needs to have nullptr set in case something changes later. It needs to be possible to tell if a pointer is deleted or not. The only way is setting it to nullptr, just deleting it leaves it in limbo where if(ptr) checks don't return false.

Take it from experience, forgetting this will cause nightmare level problems in the future 😆

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Alright. Done in d65e03c.

Based on a Chow-Liu tree approximation to Karto's graph
upon marginalization, using Karto's solver estimate of
the full joint information matrix to build new nonlinear
constraints.

Signed-off-by: Michel Hidalgo <[email protected]>
Signed-off-by: Michel Hidalgo <[email protected]>
@hidmic hidmic force-pushed the noetic/feature/chow_liu_tree_approximation branch from 295cce4 to c119840 Compare March 25, 2022 21:51
- Use a boost::bimap for node id <-> parameter block lookup
- Use a boost::pool for parameter block allocation
- Change Karto::Solver graph getter signature

Signed-off-by: Michel Hidalgo <[email protected]>
Signed-off-by: Michel Hidalgo <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants