How to calculate Rotation matrix for Rotation from GPS Co-ordinate system to SLAM Co-ordinate system

Question

In our SLAM algorithm, we want to rotate GPS co-ordinates to SLAM co-ordinate system so that those GPS positions can be used for bundle adjustment in monocular SLAM (Scale correction).

We have used following procedure for rotation.

1. Convert Lat, Long, Att GPS co-ordinates to XYZ co-ordinate using Geoconverter library. (Resetting origin same as SLAM co-ordinate's origin)
2. Therefore, calculate rotation matrix between XYZ GPS co-ordinates and SLAM co-ordinates using rodrigues rotation formula for the first vector after origin.
3. Then, for each XYZ GPS position use the rotation matrix to calculate the GPS co-ordinates in SLAM co-ordinate system.

However, the results are not as expected, the GPS co-ordinate plane does not align with SLAM co-ordinates, hence bundle adjustment fails.

To check the actual position of the GPS co-ordinates after rotation, we plotted them (as shown below). Front view (GPS(olive), SLAM(blue), MapPoints(black)) Without MapPoints Side view to understand GPS and SLAM are in different plane Side view with MapPoints Side view different angle

Can you point out the mistake in the process or suggest an alternate process to align GPS data.

Thanks

Saurabh Kumar