Result of pose graph optimization same as the input pose graph

[SubMishMar]

I am trying to use the open3d implementation of posegraph optimization for optimizing the poses of the Manhattan g2o format dataset. But due to some strange reason, the output of the optimization process is identically same to the input pose graph. Can anyone tell me why is it so?

Here is the code:

import numpy as np
import sys

sys.path.append("../..")
sys.path.append("../Utility")
from py3d import *
from common import *
from opencv import *
import matplotlib.pyplot as plt
import sys

#from optparse import OptionParser

counter = 1;

pose_graph = PoseGraph()

set_verbosity_level(VerbosityLevel.Debug)

g = open('unoptimized_pose_graph.txt', 'w') # for purpose of plotting
i = 0;
with open('input_M3500_g2o.txt','r') as f:
    for line in f:
        if(line.split(' ')[0] == 'VERTEX_SE2'):
            x = float(line.split(' ')[2])
            y = float(line.split(' ')[3])
            theta = float(line.split(' ')[4])
            c = np.cos(theta)
            s = np.sin(theta)
            wTi = np.array([[ c, -s, 0, x], [ s, c, 0, y], [ 0, 0, 1, 0], [ 0, 0, 0, 1]])
            pose_graph.nodes.append(PoseGraphNode(wTi))
            g.write(str(i) + ' ' + str(x) + ' ' + str(y) + '\n')
            i = i + 1
        else:
            source_id = int(line.split(' ')[1])
            target_id = int(line.split(' ')[2])

            x = float(line.split(' ')[3])
            y = float(line.split(' ')[4])
            theta = float(line.split(' ')[5])
            c = np.cos(theta)
            s = np.sin(theta)
            trans = np.array([[ c, -s, 0, x], [ s, c, 0, y], [ 0, 0, 1, 0], [ 0, 0, 0, 1]])

            shape = (6, 6)
            info = np.zeros(shape)

            info[0][0] = float(line.split(' ')[6])
            info[1][1] = float(line.split(' ')[9])
            info[5][5] = float(line.split(' ')[11])

            # print(info)
            if abs(source_id - target_id) == 1:
                pose_graph.edges.append(PoseGraphEdge(source_id, target_id, trans, info, uncertain=False))
            else:
                pose_graph.edges.append(PoseGraphEdge(source_id, target_id, trans, info, uncertain=True))
f.close()
g.close()

print("Optimizing PoseGraph ...")

set_verbosity_level(VerbosityLevel.Debug)
method = GlobalOptimizationLevenbergMarquardt()
criteria = GlobalOptimizationConvergenceCriteria()
option = GlobalOptimizationOption(
        max_correspondence_distance = 0.03,
        edge_prune_threshold = 0.25,
        reference_node = 0)
global_optimization(pose_graph, method, criteria, option)
set_verbosity_level(VerbosityLevel.Error)

f = open('optimized_pose_graph.txt', 'w') #for purpose of plotting
for i in range(len(pose_graph.nodes)):
    x = pose_graph.nodes[i].pose[0][3]
    y = pose_graph.nodes[i].pose[1][3]
    f.write(str(i) + ' ' + str(x)+' '+str(y)+'\n')
f.close()

`

[syncle]

Can you share input_M3500_g2o.txt too? I will have a look.

Edit: Can I use this one? https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0

[syncle]

Infomation matrix should not have zero diagonal element. Otherwise, it will produce zero residual. Replacing

info = np.zeros(shape)

this with

info = np.identity(shape)

will minimize graph energy. I got this

[GlobalOptimizationLM] Optimizing PoseGraph having 3500 nodes and 5453 edges. 
Line process weight : 0.082639
[Initial     ] residual : 7.036830e+04, lambda : 1.448914e+01
[Iteration 00] residual : 3.065880e+03, valid edges : 1826, time : 979.903 sec.
[Iteration 01] residual : 2.532268e+03, valid edges : 2834, time : 981.329 sec.
:

A few notes:

• I am not sure how this file fills out information matrix. Filling only info[0][0], info[1][1], and info[5][5] looks bit unfamiliar to me
• Single iteration took very long. Open3d uses Eigen implementation of dense LDLT. This part could be improved.
• The function below takes a long time to get a determinant. This function can have a switch that skips determinant computation. https://github.com/IntelVCL/Open3D/blob/355fdf0352ab99dfeff4cce5a23e365cf153fbaf/src/Core/Utility/Eigen.cpp#L35-L51

[SubMishMar]

@syncle , yes thats the right link to the dataset I am using.

Yes, I now understand that having zeros on the diagonal of the info matrix makes no sense. I had tried replacing this by an identity matrix like you have and as you have mentioned, it took forever, so I never waited long enough to see the first iteration and just pressed ctrl+c .

As an answer to your note:

1. The data set contains 3D data, x, y and theta but open3d accepts 6D data. so, I wrote the transformations as

            x = float(line.split(' ')[2])
            y = float(line.split(' ')[3])
            theta = float(line.split(' ')[4])
            c = np.cos(theta)
            s = np.sin(theta)
            wTi = np.array([[ c, -s, 0, x], [ s, c, 0, y], [ 0, 0, 1, 0], [ 0, 0, 0, 1]])

And the information matrix as

            shape = (6)
            info = np.identity(shape)

            info[0][0] = float(line.split(' ')[6])
            info[1][1] = float(line.split(' ')[9])
            info[5][5] = float(line.split(' ')[11])

Now, I only fill info[0][0] , info[1][1] and info[5][5] because I think that they represent the variance terms for X, Y and Yaw(theta) respectively. Correct me if I am wrong.

If you look at the data set, there are two types of rows, VERTICES and EDGES and their description can be found here: (https://github.com/RainerKuemmerle/g2o/wiki/File-Format-SLAM-2D)

As I can see that you were patient enough to wait for the execution to complete, did you compare the input poses and the optimized output poses?

Could you tell me how I can fill the info matrix for 2D pose graph optimization? Also, Can I just comment that block out and re-make open3d? Will not calculating the determinant affect my results?

[SubMishMar]

@syncle Which part do you think is the major bottle neck? is it

    double det = A.determinant();

or

Eigen::MatrixXd x = A.ldlt().solve(b);

[SubMishMar]

@syncle Moreover, I dont think that that particular function you have mentioned is being called when I am doing pose graph optimization, I have put a cout flag there and I dont see any output. I searched for the files where this function has been called and they are located at https://github.com/IntelVCL/Open3D/blob/35be432d45bf47bbcdf920c62d7d9d446ac6d36d/src/Core/Registration/ColoredICP.cpp and https://github.com/IntelVCL/Open3D/blob/b30ee02042a7a490e13f8db68dd98dc25113f467/src/Core/Registration/FastGlobalRegistration.cpp And I don't thnk these files have much to do with pose graph optimization (or do they?)

[syncle]

You're right. It does not use this function. The only bottleneck is Eigen::VectorXd delta = H.ldlt().solve(b); I am looking your description about information matrix.

[syncle]

I think nodes are fine, but filling information matrix need to be carefully considered to use g2o compatible dataset in open3d.

Open3D's Pose graph optimization follows the definition of information matrix described in here: http://redwood-data.org/indoor/registration.html (please see Eq 8) The key idea is to embed point to point distance into an information matrix.

[SubMishMar]

I will check it out, thanks.

So, you say that there is some difference between the way information matrices are defined for g2o and open3d?

[syncle]

Correct. This case I haven't tried so far, and it require some study. Let's share our thoughts in this thread.

[SubMishMar]

In my opinion, one of the major differences between the formats of information matrices in open3d and g2o is that, in open3d the first three diagonal elements of the information matrix are related to rotation components and the last three to translational components,

https://github.com/IntelVCL/Open3D/blob/355fdf0352ab99dfeff4cce5a23e365cf153fbaf/src/Core/Registration/GlobalOptimization.cpp#L70-L78

But, in g2o it must be the other way, i.e., the first three diagonal elements belong to translational variables and the last three to rotational. As shown in the following links,

1. https://github.com/grisetti/g2o_frontend/blob/master/g2o_frontend/octave/t2v.m
2. https://github.com/grisetti/g2o_frontend/blob/master/g2o_frontend/octave/mat2quat.m

I found this t2v representation in a graph slam course by Dr. Cyrill Stachniss and I think this is the one implemented in g2o .

The link to the course slide is http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/pdf/slam15-ls-slam.pdf . Please check slide 27.

[syncle]

That's awesome. I also found some related bug and submitted fix in #368. Another difference is that g2o uses quaternion for representing rotational components, whereas open3d uses linear coefficients for skew symmetric matrix, which is not exact rotation matrix.

Another thing is open3d builds information matrix \sum_q [-q_x | I ]^T * [-q_x | I ]where q is not quatanion. It is from a correspondence. (please see http://redwood-data.org/indoor/registration.html).

[SubMishMar]

"whereas open3d uses linear coefficients for skew symmetric matrix, which is not exact rotation matrix"

What do you mean by this?

[SubMishMar]

Does the following code refer to the "linearized" rotational components?

https://github.com/IntelVCL/Open3D/blob/355fdf0352ab99dfeff4cce5a23e365cf153fbaf/src/Core/Registration/GlobalOptimization.cpp#L70-L78

[syncle]

Yes. Please see this https://github.com/IntelVCL/Open3D/blob/355fdf0352ab99dfeff4cce5a23e365cf153fbaf/src/Core/Registration/GlobalOptimization.cpp#L42-L50

[SubMishMar]

@syncle, I was wondering if we can provide any arbitrary information matrix (ofcourse it must be a valid information matrix, satisfying all basic properties) to the pose graph optimization algorithm in open3d or it should necessarily be generated from one of the various open3d functions which return the information matrix for eg get_information_matrix_from_point_clouds() or compute_rgbd_odometry()

[qianyizh]

Yes, I believe you can use any valid information matrix.

[SubMishMar]

While executing the reconstruction tutorial given in the open3d website on the 016 dataset I realized that the result of the final integration doesnt change if I use or dont use pose graph optimization. @syncle @qianyizh , can you please explain this? Is it because of the fact that the number of poses being optimized is pretty small?

[syncle]

We just merged a patch related to the information matrix. It will be needed to make a pose graph again. Or, It maybe just because the pose graph is much accurate than before. With the patched version, I observe the tutorial script takes just 3~5 times of LM iteration, while energy is reduced 20x smaller than the initial one.

[SubMishMar]

Interesting. The optimization works for sure, but the changes on the final integration is not prominently visible. Probably because the initial alignment algorithms work very well.

Besides, I replaced the ldlt thing by using sparse cholesky decomposition. Something like this:

    Eigen::MatrixXd H_LM = H + current_lambda * H_I;
    Eigen::SparseMatrix<double> H_LM_sparse = H_LM.sparseView();
    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> chol(H_LM_sparse);
    Eigen::VectorXd delta = chol.solve(b); 

So, now the manhattan dataset is processed pretty fast and it takes about 2 seconds per iteration:

[Initial     ] residual : 7.185224e+04, lambda : 1.291831e-01
[Iteration 00] residual : 1.481994e+03, valid edges : 14, time : 6.333 sec.
[Iteration 01] residual : 5.288676e+02, valid edges : 58, time : 2.327 sec.
[Iteration 02] residual : 2.837691e+02, valid edges : 113, time : 2.221 sec.
[Iteration 03] residual : 1.859188e+02, valid edges : 215, time : 2.229 sec.
[Iteration 04] residual : 1.387585e+02, valid edges : 378, time : 2.231 sec.
[Iteration 05] residual : 1.039180e+02, valid edges : 588, time : 2.239 sec.
[Iteration 06] residual : 6.296705e+01, valid edges : 785, time : 2.293 sec.
[Iteration 07] residual : 3.927089e+01, valid edges : 1022, time : 2.264 sec.
[Iteration 08] residual : 2.727294e+01, valid edges : 1211, time : 2.249 sec.
.....

(earlier it used to take 500 seconds), but I still dont understand how to input the information data available in manhattan dataset into open3d.

[syncle]

WOW, this is really awesome. Thanks Subodh! You can submit your changes for the better Open3D :)

[SubMishMar]

Okay, I will submit a PR later today. Thanks :)

[SubMishMar]

This issue needs to be closed as the "Result of the pose graph optimization was same as the input" only because there were not enough loop closure edges added to the graph. Adding enough loop closure edges will give a different result (which may not always be the best).

But, the other issues mentioned in the above thread still needs to be solved.

[szx0112]

Is it possible for us to view the pose graph optimization process (such as iterations of cost reduction), so we can understand the error really decreasing?

[griegler]

@szx0112 could you please open a new issue with your question.