Does anyone know how to add GPS coordinates in optimization of monocular ORBSLAM?

[nikhilarane]

[nikhilarane]

I am thinking an error function of (X - Xgps)^2 + reprojection error. What will be the structure for this?

[RainerKuemmerle]

How about modelling GPS by an EdgeXYZPrior

[shanmukhananda]

You can try below approach.

• create a new custom unary edge which implements BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap>. This is a prior edge
• you can also constrain the camera nodes based on GPS distance for this you have to create custom binary edge which implements BaseBinaryEdge<1, double, VertexSE3Expmap, VertexSE3Expmap>. Error will be distance(camera_pos) - distance(gps_pos).
• add the prior edge (and/or binary distance edge) in global bundle adjustment and local bundle adjustment.
• adding in global bundle adjustment fixes initial scale issue(in monocular case). adding these edge in local bundle adjustment improves the trajectory and fixes the scale drift.
• setting correct information matrix is crucial.
• to use prior edge you will have to convert gps coordinate to local.

[nikhilarane]

Thanks, Rainer and Shanmukhananda.

"create a new custom unary edge which implements BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap>. This is a prior edge"

What will be the Jacobian for this edge

[sume-cn]

You can try below approach.

• create a new custom unary edge which implements BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap>. This is a prior edge
• you can also constrain the camera nodes based on GPS distance for this you have to create custom binary edge which implements BaseBinaryEdge<1, double, VertexSE3Expmap, VertexSE3Expmap>. Error will be distance(camera_pos) - distance(gps_pos).
• add the prior edge (and/or binary distance edge) in global bundle adjustment and local bundle adjustment.
• adding in global bundle adjustment fixes initial scale issue(in monocular case). adding these edge in local bundle adjustment improves the trajectory and fixes the scale drift.
• setting correct information matrix is crucial.
• to use prior edge you will have to convert gps coordinate to local.

I have the same question, how to compute the Jacobian Matrix? thanks a lot!

[sume-cn]

Solved, just leave linearizeOplus() not implemented. BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap> works, but some tricks must be used to get a fine trajectory.

[radeonwu]

Solved, just leave linearizeOplus() not implemented. BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap> works, but some tricks must be used to get a fine trajectory.

Hi, I followed shanmukhananda 's suggestion to add GPS absolute observation of XYZ, into ORB-SLAM2's mono operation pipeline. Below is what I did

• defined a custom edge type EdgeSE3ExpmapXYZPrior() : BaseUnaryEdge<3, Vector3, g2o::VertexSE3Expmap>(), leaving linearizeOplus unimplemented
• added the above edges in both LocalBundleAdjustment and GlobalBundleAdjustment, and information is set to identity() As a result, the modified ORB-SLAM2 program can run to finish map building, but the result is more or less the same as before adding the GPS constraints, especially for the map scale which is far less than the ground truth.

Update: I also tried increase the information matrix value for the self-defined prior edge constraint in LocalBundleAdjustment, e.g., *= 10000, then the optimizer seems could not converge. Below is some output from the terminal, ... iteration= 0 chi2= 12233271.258505 time= 0.00325631 cumTime= 0.00325631 edges= 3344 schur= 1 lambda= 2276047971.182659 levenbergIter= 7 iteration= 1 chi2= 12233269.916132 time= 0.000874424 cumTime= 0.00413073 edges= 3344 schur= 1 lambda= 1517365314.121773 levenbergIter= 1 iteration= 2 chi2= 12233268.823818 time= 0.000857645 cumTime= 0.00498838 edges= 3344 schur= 1 lambda= 1011576876.081182 levenbergIter= 1 iteration= 3 chi2= 12233268.696784 time= 0.00132654 cumTime= 0.00631492 edges= 3344 schur= 1 lambda= 1348769168.108242 levenbergIter= 2 iteration= 4 chi2= 12233268.696784 time= 0.00274892 cumTime= 0.00906384 edges= 3344 schur= 1 lambda= 241371644453251104.000000 levenbergIter= 8 iteration= 0 chi2= 12232875.569518 time= 0.00539925 cumTime= 0.00539925 edges= 3313 schur= 1 lambda= 58654483926809133056.000000 levenbergIter= 10 ...

Any comments or suggestions to improve will be welcome.

Wu

[whu-lyh]

@radeonwu have u figure it out or not?