Discontinuities in the Pose Graph

[VladimirYugay]

🐛 Describe the bug

I use pose graph implementation and optimization from the examples.

When optimizing my own pose graph with an identity information matrix for the neighboring nodes, the resulting graph can have discontinuities. Under discontinuity, I mean really strong jumps between consecutive nodes in the graph.

On the attached picture you can see the ground-truth graph (GT), estimated graph, and a graph optimized with pypose. There's a huge jump at the very bottom of the image (6 dots)

When running PGO with G2O, there are no discontinuities like that.

I also checked that optimizers used in both G2O and pypose are the same.

Do you know any possible reason for such behavior?

Versions

Pypose version: 0.4.4

[wang-chen]

Hi, it would be great if you could provide your scripts and data so that we can reproduce your results. I am guessing that some hyperparameter tuning may be able to solve this problem.

[VladimirYugay]

Thanks for your response. Will provide the repro scripts + data soon.

Another question on that note, How can I freeze some nodes during optimization? For example, nodes A and B are connected by an edge, but I know that node A is a ground-truth and I don't want to change it. However, I'm still interested in changing B based on the constraint between A and B.

[wang-chen]

Hi, given the use case, you have multiple options.
(1) You only need to define model.B as pp.Parameter and define model.A as normal torch.Tensor or pp.LieTensor.
(2) Only provide model.B to optimizers, e.g., optim = pp.optim.LM(model.B, ...) or optim = torch.optim.SGD(model.B.parameters(), ...).

[VladimirYugay]

For the 1) option, it is still a little bit unclear. My forward function looks like this:

    def forward(self, edges, poses):
        node1 = self.nodes[edges[..., 0]]
        node2 = self.nodes[edges[..., 1]]
        error = poses.Inv() @ node1.Inv() @ node2
        return error.Log().tensor()

Now if I want to make a node with id = i static. It also has an edge over another node with id = j which I want to optimize based on that edge.

Now self.nodes = pp.Parameter(pp.SE3(cur_nodes.float())). How can I make the node at index i static in that case?

[FuTaimeng]

Hi. How many static nodes do you have?

1. If only one (e.g. a common demand is to fix the first node), you can consider first solve the PGO, then transform the whole graph back (calculate the displacement of the static node caused by PGO, and transform all nodes use the inverse of this transformation).
2. If more than one, I'd recommend using two tensors, one for static nodes (model.B) and the other for the nodes to be optimized (model.A). Then you can follow the previous answer:

Hi, given the use case, you have multiple options. (1) You only need to define model.B as pp.Parameter and define model.A as normal torch.Tensor or pp.LieTensor. (2) Only provide model.B to optimizers, e.g., optim = pp.optim.LM(model.B, ...) or optim = torch.optim.SGD(model.B.parameters(), ...).

[FuTaimeng]

BTW, in the provided figure, seems the jump is also in the original trajectory before optimization? So this bug probably has nothing to do with PGO, and I'd recommend to double check the input data.

[VladimirYugay]

Hey there. I can have more than one static node.

1. It will not work even for one static node. This will lead to discontinuities. Since the error is distributed over all the nodes, putting the static one back will lead to the trajectory break.

2. Can you please elaborate? I've provided the code snippet above of my graph forward path. How can I do that in that snippet specifically?

3. Regarding the jump in the trajectory - it's not a bug. It is another trajectory that is disconnected from the first one.

[FuTaimeng]

Thanks for your reply.

1. It won't break the trajectory. As I said, you need to transform all nodes using the same transformation. Here is an example: https://github.com/sair-lab/iSLAM/blob/main/pvgo.py
2. I originally thought that the bug is you have 6 discontinue points at the bottom left of the figure. If this is not the case, which segment is the discontinuity you referred? The other part of the trajectory looks smooth to me.

[VladimirYugay]

Thanks for such a quick response.

1. Thanks, I will have a look. Another thing I was wondering, how in g2o it is allowed to have several static nodes by just passing the is_fixed flag when a node is created. And what would it take to have it "natively" at pypose.

2. Under discontinuity I mean the big jump that those 6 points at the bottom made. After optimization, those 6 points became so different from the original 6 points.

[FuTaimeng]

Hi,

1. Sorry we do not have this feature now. However, we will soon release the constrained optimizers and they would be helpful for this problem.
2. I got your point. Yes, the relative location between the two trajectories can change, as there's no constraint between them. You may consider to fix the starting point of both trajectories to solve this issue.

[wang-chen]

Hi @VladimirYugay,
We just released a new usage for 2nd-order optimizers in v0.6.2. You may update it by pip install -U pypose. It is fully backward compatible.
Basically, you can now return multiple error terms in your model to be optimized. For your case, you may simply return the diff with the ground truth if some specific elements have a fixed value.

        class Model(nn.Module):
            def __init__(self, *dim):
                super().__init__()
                self.pose = pp.Parameter(pp.randn_SE3(*dim))

            def forward(self, poses):
                error1 = ...
                error2 = self.pose[0] - groundtruth
                return error1, error2

For a runnable use case, refer to this test example.

Note that this is not constraint optimization, this is just an API to augment the error terms. Constraint optimization will be in the next major release.