Dijkstra nodes estimation in CGraphSlamEngine overwrites previously optimized values

[YB27]

@bergercookie

Hi,
I am currently using MRPT 1.5.6 and more specifically the CGraphSlamEngine class.

In this class, the graph is optimized (CGraphSlamEngine_impl.h l. 615) and, if a new node has been previously registered, node values are estimated by first converting the graph into a MST (minimum spanning tree) using the dijkstra algorithm (CGraphSlamEngine_impl.h l. 660, execDijkstraNodesEstimation()).

Doing so, it appears that if a new node has been registered, the optimized values are overwritten using solely the computed MST and the edge constraint (ie here relative poses between nodes).

It seems that the function execDijkstraNodesEstimation() should only be called if no graph optimization occurred.

Can you confirm ?

Thanks in advance

[bergercookie]

Hi @YB27,

I'm afraid it's been a long time since I last worked on that class so take this with more like a suggestion rather than an authoritative answer.

Doing so, it appears that if a new node has been registered, the optimized values are overwritten using solely the computed MST and the edge constraint (ie here relative poses between nodes).

You mean the optimised values for the rest of the nodes in the graph, right?

In general, I do think that the optimisation should supersede the MST estimation and also once the position of a node has been estimated there should be no reason to rerun MST, at least not for that node.

It seems that the function execDijkstraNodesEstimation() should only be called if no graph optimization occurred.

I'm not sure if that's the right way as graph optimisation (at least partial) is happening in every single GraphSLAM step.

Overall I think the problem stems from the fact that on every new graph node registration I rerun the graph_poses_dijsktra_init function. A better approach would be to implement a graph_poses_dijkstra_update function and call that instead. The latter would be responsible for estimating the global coords for just the newly added node(s) instead of the whole graph and would't tamper with the pose estimation of the already optimised nodes.

Does that make sense?

[YB27]

Thank for your answer !

It is exactly what I was thinking.
But in the case where a new node is registered and an optimization previously occurred, the new node would already have its global pose estimation computed so there is no need to run anything else ?

Thus I think that graph_poses_dijkstra_update() should only be responsible for estimating the global coords of the newly added node(s) which were excluded from the previous GSO update.

[bergercookie]

Thus I think that graph_poses_dijkstra_update() should only be responsible for estimating the global coords of the newly added node(s) which were excluded from the previous GSO update.

Sounds accurate, yes.

Can you take a look and see if the discussed changes work in practice?
I'm happy to review any Pull-Request on this matter.

[YB27]

I will try to look into it ! Thanks for the help.

[bergercookie]

I'd keep this issue open since this looks like a bug of the current implementation