Cloudpoints extracted from map.msg have different Centers and different World positions

[joavd]

This issue is somewhat related to issue #434 .

After running run_image_slam,the output from map.msg gives me different centers and world positions for Landmarks and Keyframes. This shouldn't happen, since the data from the video is gathered from the same source, and PangolinViewer even displays it correctly.

Procedure:

1. run_image_slam
2. Extract Keypoints and Landmarks
3. Open them on Open3D or Meshlab

In this example it doesn't really matter that it doesn't have the same center nor the same world positions, since it's easy to just translate it and rotate it.
However the problem lies in each different video that's run through OpenVSLAM. In this video it needs to be rotated and translated by X, however on another video it has a completely different center/position, so it needs to be rotated and translated by Y. On another video it will have to be by Z etc.

[ymd-stella]

This issue is caused by a lack of understanding of map.msg and the data in it.
This is not an OpenVSLAM issue.
If you want to treat it as an OpenVSLAM issue, you should propose a feature request to document the use of map.msg.
Otherwise, it should be handled at https://spectrum.chat/openvslam.

[joavd]

This issue is caused by a lack of understanding of map.msg and the data in it.

Yes, it is certainly caused by a lack of understanding of the output file, since I'm new to openvslam.

This is not an OpenVSLAM issue.

But is that the normal usage of map.msg? You have to "hardcode" the extracted data in order for it to have the same position and center in the world?
Because right now that's what it happens, extracting Landmarks and Keyframes the same way lead to different positions, which in itself seems weird since they were recorded/written the same way.
I'll post on spectrum, but it's useful to have things written here as well, in order to be easily found by the next people who might have a similar problem.

If you want to treat it as an OpenVSLAM issue, you should propose a feature request to document the use of map.msg.

Issue opened as suggested, on #479

[ymd-stella]

Are you under the impression that map.msg is output to be read by an external application?
I believe map.msg is just an intermediate save and was created with the intention of loading it in openvslam. If that's true, then there's nothing wrong with it as long as it can be loaded in openvslam.
(Sorry. This should have been on spectrum.chat. Let's discuss the rest on spectrum.chat.)

[joavd]

I'm not under any impression, I'm just trying to read data from the file, and the data has different settings.
Right now, as of my understanding from reading the docs, the only way to "export" the data is through the generated map.msg file, so it makes sense that data inside it should be consistent at least.
For eg. if it's a normal behaviour for one of the things to be upside down and with a translation of +10 units in x, then the output from different videos should all be upside down with a translation of +10 in x as well. (Not each one having different rotations and translations)

[joavd]

After a few days of testing I can confirm the data extracted from map.msg is different from the one shown on PangolinViewer. Either OpenVSLAM reads the map.msg and changes it's position/orientation/rotation, or the data saved to the map.msg file is not consistent.

I'm extracting data from map.msg using Python:

# Keyframes
import msgpack
import numpy as np
import open3d as o3d
import pydot
from scipy.spatial.transform import Rotation as R

# ...

with open(bin_fn, "rb") as f:
        data = msgpack.unpackb(f.read(), use_list=False, raw=False)  

# The point data is tagged "keyframes"
key_frames = data["keyframes"]
print("Point cloud has {} points.".format(len(key_frames)))  

key_frame = {int(k): v for k, v in key_frames.items()}

# ...

l=[]
for keyf in sorted(key_frame.keys()):
    point = key_frame[keyf]        

    # Get conversion from camera to world
    trans_cw = np.matrix(point["trans_cw"]).T
    rot_cw = R.from_quat(point["rot_cw"]).as_matrix()

    #Computer conversion from world to camera
    rot_wc = rot_cw.T
    trans_wc = rot_wc * trans_cw​

    l.append(trans_wc)

l.sort(key=lambda k: k[0])
# Creates a Open3D object based on the data extracted and saves it as a .ply file
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(l)
o3d.io.write_point_cloud(dest_ply, pcd)

# Landmarks
import msgpack
import numpy as np
import open3d as o3d

# ...

with open(bin_fn, "rb") as f: 
        data = msgpack.unpackb(f.read(), use_list=False, raw=False)

# The point data is tagged "landmarks"
landmarks = data["landmarks"]
print("Point cloud has {} points.".format(len(landmarks)))

l = []
for id, point in landmarks.items():
    pos = point["pos_w"]
    l.append(pos)

l = np.asarray(l)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(l)
o3d.io.write_point_cloud(dest_ply, pcd)

After the data is extracted, I just merge the files in order to see if they have a similar representation as in PangolinViewer, however both Keyframes and Landmarks are always upside down. For which I managed to orientate them accordingly. And then the Keyframes also need to be rotated by 180 degrees on the Y axis. (turned around)

import open3d as o3d

# ...

pcd = o3d.io.read_point_cloud("./PointCloud.ply")
cc = o3d.io.read_point_cloud("./CameraCenters.ply")

# Rotations use Euler angles
pcd.rotate(pcd.get_rotation_matrix_from_xyz((np.pi, 0, 0)), center=(0,0,0))
cc.rotate(cc.get_rotation_matrix_from_xyz((np.pi, 0, 0)), center=(0,0,0))
cc.rotate(cc.get_rotation_matrix_from_xyz((-np.pi / 10, 0, np.pi)), center=(0,0,0))

# Visualize on 3D viewer
o3d.visualization.draw_geometries([pcd + cc])

This managed to work on Video Example 1 (completely centered and positioned), on Video Example 2 the pointclouds have the correct rotation but don't have the same center (need to be translated).
This shouldn't happen since data is being extracted in the exact same way. Previous discussion here

Edit: updated the Keyframes code extractor with the solution from @ymd-stella , but the original issue still remains (both Keyframes and Landmarks are upside-down and don't share the same center)

[joavd]

Much appreciated for the help, @ymd-stella found a solution for this problem. His approach can be found here, and can be useful to anyone that wants to extract the data from the output map.msg.

Getting the Keyframes positions to match the orientation and center of the Landmarks requires explicitly extracting the position, as seen on stella's example:

with open("map.msg", "rb") as f:
    u = msgpack.Unpacker(f)
    msg = u.unpack()

keyfrms = msg["keyframes"]
landmarks = msg["landmarks"]

keyfrms_tum = []
keyfrm_points = []
landmark_points = []
for keyfrm in keyfrms.values():
    # get conversion from camera to world
    trans_cw = np.matrix(keyfrm["trans_cw"]).T
    rot_cw = R.from_quat(keyfrm["rot_cw"]).as_matrix()
    # compute conversion from world to camera
    rot_wc = rot_cw.T
    trans_wc = - rot_wc * trans_cw
    keyfrm_points.append((trans_wc[0, 0], trans_wc[1, 0], trans_wc[2, 0]))
    keyfrms_tum.append((keyfrm["ts"], trans_wc.tolist(), R.from_matrix(rot_wc).as_quat().tolist()))
keyfrm_points = np.array(keyfrm_points)

landmark_points = []
for lm in landmarks.values():
    landmark_points.append(lm["pos_w"])
landmark_points = np.array(landmark_points)

keyfrms_tum.sort(key=lambda k: k[0])

With this, both Keyframes and Landmarks match the correct position, as seen in PangolinViewer when running openvslam.
The final result is still turned upside down, but this can be easily fixed by rotating both point clouds by 180 degrees, or by PI radians on the X axis.