Add node marginalization support #468

hidmic · 2022-01-14T14:05:22Z

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.

SteveMacenski · 2022-01-14T19:31:54Z

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.

slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h

hidmic · 2022-01-14T20:20:04Z

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 · 2022-01-14T22:12:47Z

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 · 2022-01-30T02:30:00Z

@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

SteveMacenski · 2022-03-17T18:49:11Z

slam_toolbox/solvers/ceres_solver.cpp

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

  // formulate problem
-  angle_local_parameterization_ = AngleLocalParameterization::Create();
+  local_parameterization_ = new ceres::ProductParameterization(


Please explain this

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.

https://github.com/ceres-solver/ceres-solver/blob/b0aef211db734379319c19c030e734d6e23436b0/docs/source/nnls_modeling.rst#classproductparameterization I noticed there is a depreciation warning, should we be using something else instead?

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.

SteveMacenski · 2022-03-17T18:50:49Z

slam_toolbox/solvers/ceres_solver.cpp

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


Please explain this

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.

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

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?

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.

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

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

SteveMacenski · 2022-03-21T20:08:25Z

slam_toolbox/solvers/ceres_solver.cpp

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

  // formulate problem
-  angle_local_parameterization_ = AngleLocalParameterization::Create();
+  local_parameterization_ = new ceres::ProductParameterization(


This is newed but never deleted in reset / destructor

Ahh, good catch. Fixed in aa9ffa6.

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.

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

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

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

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.

slam_toolbox/solvers/ceres_solver.hpp

Lines 72 to 73 in 646ef41

std::unordered_map<int, Eigen::Vector3d> * nodes_;

std::unordered_map<size_t, ceres::ResidualBlockId> * blocks_;

Anyhow, it's orthogonal to this patch.

SteveMacenski · 2022-03-21T20:14:08Z

slam_toolbox/solvers/ceres_solver.cpp

-  sqrt_information(2,2) = precisionMatrix(2,2);
-
+  Eigen::Matrix3d sqrt_information =
+    pLinkInfo->GetCovariance().Inverse().ToEigen().llt().matrixL();


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

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

Isbackported() {return true;};

Indeed. I've rebased this PR since.

SteveMacenski · 2022-03-21T20:17:46Z

slam_toolbox/lib/karto_sdk/src/Mapper.cpp

+              edge->GetSource()->GetObject()->GetUniqueId(),
+              edge->GetTarget()->GetObject()->GetUniqueId());
+          m_pGraph->RemoveEdge(edge);
+          delete edge;


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

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?

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 😆

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]>

- 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]>

hidmic changed the title ~~Add node marginalization support.~~ Add node marginalization support Jan 14, 2022

hidmic mentioned this pull request Jan 14, 2022

[Rolling] Implement pose graph node marginalization ekumenlabs/slam_toolbox#3

Closed

SteveMacenski reviewed Jan 14, 2022

View reviewed changes

slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h Outdated Show resolved Hide resolved

hidmic requested a review from SteveMacenski February 1, 2022 18:11

SteveMacenski reviewed Mar 17, 2022

View reviewed changes

SteveMacenski reviewed Mar 21, 2022

View reviewed changes

hidmic requested a review from SteveMacenski March 25, 2022 21:30

hidmic added 4 commits March 25, 2022 18:47

Add node marginalization support.

5411136

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]>

Use LGPL 2.1 in lieu of LGPL 3

4bc5903

Signed-off-by: Michel Hidalgo <[email protected]>

Delete CeresSolver allocations upon destruction/reset

aa9ffa6

Signed-off-by: Michel Hidalgo <[email protected]>

Drop duplicate edge checks from karto::Mapper

c119840

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic force-pushed the noetic/feature/chow_liu_tree_approximation branch from 295cce4 to c119840 Compare March 25, 2022 21:51

hidmic added 2 commits April 3, 2022 23:32

Rework parameter/residual block handling in Ceres solver

1041a52

- 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]>

Null edge pointer after delete

d65e03c

Signed-off-by: Michel Hidalgo <[email protected]>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add node marginalization support #468

Add node marginalization support #468

hidmic commented Jan 14, 2022 •

edited

Loading

SteveMacenski commented Jan 14, 2022 •

edited

Loading

hidmic commented Jan 14, 2022

SteveMacenski commented Jan 14, 2022

hidmic commented Jan 30, 2022

SteveMacenski Mar 17, 2022

hidmic Mar 18, 2022 •

edited

Loading

SteveMacenski Mar 21, 2022

hidmic Mar 25, 2022

SteveMacenski Mar 17, 2022

hidmic Mar 18, 2022 •

edited

Loading

SteveMacenski Mar 21, 2022

hidmic Mar 25, 2022

hidmic Apr 4, 2022

SteveMacenski Apr 4, 2022

hidmic Apr 4, 2022

SteveMacenski Mar 21, 2022

hidmic Mar 25, 2022 •

edited

Loading

hidmic Apr 4, 2022

SteveMacenski Apr 4, 2022

hidmic Apr 4, 2022

SteveMacenski Apr 4, 2022

hidmic Apr 7, 2022

SteveMacenski Mar 21, 2022

hidmic Mar 25, 2022

SteveMacenski Mar 25, 2022

hidmic Apr 4, 2022 •

edited

Loading

SteveMacenski Mar 21, 2022

hidmic Mar 25, 2022

SteveMacenski Mar 25, 2022 •

edited

Loading

hidmic Apr 4, 2022

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

Add node marginalization support #468

Are you sure you want to change the base?

Add node marginalization support #468

Conversation

hidmic commented Jan 14, 2022 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

SteveMacenski commented Jan 14, 2022 • edited Loading

hidmic commented Jan 14, 2022

SteveMacenski commented Jan 14, 2022

hidmic commented Jan 30, 2022

Choose a reason for hiding this comment

hidmic Mar 18, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic Mar 18, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic Mar 25, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic Apr 4, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Mar 25, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic commented Jan 14, 2022 •

edited

Loading

SteveMacenski commented Jan 14, 2022 •

edited

Loading

hidmic Mar 18, 2022 •

edited

Loading

hidmic Mar 18, 2022 •

edited

Loading

hidmic Mar 25, 2022 •

edited

Loading

hidmic Apr 4, 2022 •

edited

Loading

SteveMacenski Mar 25, 2022 •

edited

Loading