Customized confidence for edges in pose graph.

[lvgeng]

Currently the edges in pose graph is divided into odometry and loop closure by uncertainty, which is good for a RGBD reconstruction system.

However is it possible to make the certainty of edges a more flexible attribute? Sometime it just feels that it is not enough to just use two different groups.

[qianyizh]

Can you elaborate on your use case?

[lvgeng]

It is a surface scanning system (Works in a SLAM way, just like the reconstruction system in Open3D. The difference is the camera pose is provided by sensors.) in which the orientation of each point cloud is much more reliable than the position, I wish to be able to set customized error function and optimising objective function for that. I hope to be able to set the confidence of edges (since the edges between distant nodes should be less reliable than the ones close to each other.) Also, I notice that the pose graph optimization part is based on g2o, is there a python boding for it? https://github.com/uoip/g2opy which says "this project doesn't support writing user-defined types in python, but the predefined types are enough to implement the most common algorithms, say PnP, ICP, Bundle Adjustment and Pose Graph Optimization in 2d or 3d scenarios. "

I assume it is not a good solution, cause no user-defined types means no self define error function at all...

[qianyizh]

I think you can set the information matrix to give a heavier constraint on rotation than on translation.

However, for general graph optimization, I would still recommend g2o. It's a great library, :). We would have used it in Open3D if its backend were not SuiteSparse (which causes lots of pain to install cross-platform-ly and has a GPL-like license).

[lvgeng]

I think you can set the information matrix to give a heavier constraint on rotation than on translation.

However, for general graph optimization, I would still recommend g2o. It's a great library, :). We would have used it in Open3D if its backend were not SuiteSparse (which causes lots of pain to install cross-platform-ly and has a GPL-like license).

Thank you! Em.... I will keep that in mind. About the information matrix. That might be the most confusing thing for me. It is a 6x6 matrix in open3D and I read part of the g2o examples it is also a 6x6 matrix. But it is kind of hard for me to understand how it works. (Well, for instance, if I need some to give a heavier constraint on rotation than on translation, which elements I should be adjusting in the information matrix?) I tried to look into some documents but I failed to find a detailed instruction for it. (And the word "information" is kind of too general to be found through google...)

Would you mind to recommend some documents that I can read to get a better understanding of the information matrix?

After all, I wish to use Open3D cause it has amazing python APIs... (Does g2o have a good python bonding?) PS: in the paper "Robust reconstruction of indoor scenes", g2o was used for optimization. Does Open3D use different method for optimization? Based on any libs, or its own solution?

PSS: For some constraints... I might wish to add some fixed vertices in the pose optimization. LIke a group of known landmarks. Is it possible in Open3D? (I know the reference node would be fixed, but it seems that there is only one reference node?)

[qianyizh]

Information matrix

Assume you have the correct alignment already, and adds a small perturbation to it in the form of x = (alpha, beta, gamma, a, b, c)^T, then given information matrix M, the error is measured as x^T M x

For example, in this page, I have an information matrix defined for points alignment case.

You can add to this matrix an additional diagonal matrix to further constrain it, e.g.,

w 0 0 0 0 0
0 w 0 0 0 0
0 0 w 0 0 0
0 0 0 v 0 0 
0 0 0 0 v 0
0 0 0 0 0 v

If w>v then you constrain rotation more than translation.

I got the name information matrix from the 2012 g2o paper. I think it is just the inverse of the covariance matrix.

"Robust reconstruction of indoor scenes"

Back then I wrote everything in C++ and used g2o. Later on I really dislike the way g2o was wrapped so we re-implemented graph optimization in Open3D. The underlying math is the same, just different implementation.

Fixed vertices in the pose optimization

You can try to connect these nodes with odometry edges. In this case, the relative transformations between them are fixed (or at least heavily constrained).

[lvgeng]

Sorry that I came back with more questions....

For example, in this page, I have an information matrix defined for points alignment case.

Tell me if I am wrong... do you mean the "Lambda_ij*" is actually the information matrix?

(alpha, beta, gamma, a, b, c)^T

Does it mean the alpha, beta, gamma part can be treated as Euler angles?

If w>v then you constrain rotation more than translation.

w1   0    0    0    0    0
0    w2   0    0    0    0
0    0    w3   0    0    0
0    0    0    v1   0    0 
0    0    0    0    v2   0
0    0    0    0    0    v3

So... actually I can set the 3 "w" and 3 "v" in the additional matrix individually to make more flexible constraints? for instance if I decided that the rotation around z axis (in the local connection) is super reliable, I can just set a very large value for w3? More offen, if it is not just weight, but also a range constraint? Like is the x translation x_offset < x_range, then the wight is relatively low, but as long as x_offset > x_range, the error function value became very large?

PS: according to my testing results the value of information matrix elements varies in a very wide range, from exactly 2.0 to 253702.342.... (These are only the diagonal elements. Others are more crazy...) I think an identity 6x6 matrix means that the edge is very flexible and soft compared with other connections, but what does a 2.0 mean? (Well... sometime I got it really frequently.)

The underlying math is the same, just different implementation.

Good to know... Thank you! Em.. just wish Open3D can provide more flexibility in the future.

You can try to connect these nodes with odometry edges. In this case, the relative transformations between them are fixed (or at least heavily constrained).

Em... this is a problem. Currently I set the preference to about 30, which I think means that I treat loop closures much more reliable than odometry edges. But I guess I can add different "w" and "v" for land mark nodes than the actual odometry nodes? Another concern is that there is a connection checking in before the pose graph optimization. If I set the edges between land marks to be odometry connections, does it mean that there should be at least one odometry connection between these land marks and the real point cloud nodes? More general, odometry edges are automatically connected as a long chain in SLAM system. but do they have to be a 1D structure? Could there be a structure like a binary tree?

[qianyizh]

Information matrix

Your explanation is roughly correct. Basically it defines how the six parameters are connected.

Odometry edges

I forgot the detailed implementation. I think it is required that the odometry edges form one disconnected component. But it can be either a chain, or a tree, or even a loop.

[qianyizh]

Also adding @syncle to this thread.

[lvgeng]

Information matrix

Your explanation is roughly correct. Basically it defines how the six parameters are connected.

Em... I shall try. If I have a self-defined error function, how to generate the information matrix? And kind of curious how does open3D generate information matrix from two point clouds and the local transformation?

Odometry edges

I forgot the detailed implementation. I think it is required that the odometry edges form one disconnected component. But it can be either a chain, or a tree, or even a loop.

No harm to try it I guess. Will be doing so. With current version of open3D it seems to be the best choice.

[lvgeng]

Tried to use an information matrix "virtual_node_information": [ [ 100000000,-100000000, 0, 0, 0,-100000000], [-100000000, 100000000, 0, 0, 0, 100000000], [ 0, 0, 100000000, 100000000,-100000000, 0], [ 0, 0, 100000000, 100000000, 0, 0], [ 0, 0,-100000000, 0, 100000000, 0], [-100000000, 100000000, 0, 0, 0, 100000000]

For all the nodes work as land marks. this is for the edges link them to the reference point. Does not help much.... At least this matrix is not enough to fix the node position.

[barnjamin]

I've used sequential GPS coordinates to inject edges into the pose graph successfully.

I assign GPS readings to node ids using timestamps, use the translation between GPS coordinates as edge transformation and for the information matrix just something like:

info = np.identity(6)
info[:3,:] *= quality_of_gps_reading

where only the top half of the information matrix is multiplied to constrain the edge to influencing translation

[lvgeng]

I've used sequential GPS coordinates to inject edges into the pose graph successfully.

I assign GPS readings to node ids using timestamps, use the translation between GPS coordinates as edge transformation and for the information matrix just something like:

info = np.identity(6)
info[:3,:] *= quality_of_gps_reading

where only the top half of the information matrix is multiplied to constrain the edge to influencing translation

Well... that is almost what I was doing. But the point is, I cannot freeze the pose of multiple nodes no matter what my current information matrix is. Translation between GPS coordinate would add some accumulating error to the results.

[barnjamin]

what is the transformation you're using for the edge?

you have a landmark right? and you know with high accuracy the node => landmark transformation?

is your transformation the computed distance between edge's source/target nodes using the node to landmark transformation?

[lvgeng]

what is the transformation you're using for the edge?

you have a landmark right? and you know with high accuracy the node => landmark transformation?

is your transformation the computed distance between edge's source/target nodes using the node to landmark transformation?

Currently my pose graph is like this.

Initializing the Nodes and GPS nodes with same pose, which is the GPS pose.(with orientation.) The transformation set to reference edges are calculated from GPS pose, the information is set to be the matrix above.

I wish to freeze the GPS nodes during the optimization.

Odometry edges are set with computer vision estimated transformation, and information matrix generated with function

get_information_matrix_from_point_clouds()

[barnjamin]

I see.

My knowledge is limited in this subject but they way I did it was that only the frames were nodes and the edges were from RGBD Odom + computed edges based on translation between gps points associated to nodes transformed into the cameras coordinate system.

This let the optimizer figure out how to adjust the nodes (frames) poses to account for the gps measured translation.

[lvgeng]

I see.

My knowledge is limited in this subject but they way I did it was that only the frames were nodes and the edges were from RGBD Odom + computed edges based on translation between gps points associated to nodes transformed into the cameras coordinate system.

This let the optimizer figure out how to adjust the nodes (frames) poses to account for the gps measured translation.

Em... Just curious, how do you deal with accumulating error in this way? Especially when the GPS data is not so reliable.

[barnjamin]

Since I was doing road reconstruction I could snap the noisy gps coordinates to likely roads using OSRM. Instead of using the raw GPS data I used road segments.

Recently I got a Zed-F9P and by applying corrections provided by an NTRIP service I can get a cm level fix. This is sufficiently accurate for what I'm doing.

[oliver-batchelor]

I've used sequential GPS coordinates to inject edges into the pose graph successfully.

I assign GPS readings to node ids using timestamps, use the translation between GPS coordinates as edge transformation and for the information matrix just something like:

info = np.identity(6)
info[:3,:] *= quality_of_gps_reading

where only the top half of the information matrix is multiplied to constrain the edge to influencing translation

Curious if anyone has more information about how to do this? In this example what units is "quality_of_gps_reading" is it the variance (or 1/variance) for that axis?

[barnjamin]

I'm really not sure what i was referring to there but the project source is here https://github.com/barnjamin/streetsmarts

feel free to dig in, i havent worked on it in some time and never really finished it