Viewing landmarks and keyframes in Open3D

[Shitikantha-Bagh]

Hello , I had followed this discussion xdspacelab/openvslam#477

As @joavd notified "The final result from @ymd-stella 's provided code read_msg_open3d_plot.py was still turned upside down, and could be easily fixed by rotating both point clouds by 180 degrees, or by PI radians on the X axis."

I tried to visualize the keyframes trajectory with the above script and found out it to be true. I implemented the below fix by adding few lines to @ymd-stella 's script.

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)
#print(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])
for keyfrm in keyfrms_tum:
print("{} {} {} {} {} {} {} {}".format(keyfrm[0], keyfrm[1][0][0], keyfrm[1][1][0], keyfrm[1][2][0], keyfrm[2][0], keyfrm[2][1], keyfrm[2][2], keyfrm[2][3]))

----Inserted this
rotation_matrix = [[1,0,0],[0,-1,0],[0,0,-1]]
rnp = np.array(rotation_matrix)
keyfrm_points = np.dot(keyfrm_points,rnp)
landmark_points = np.dot(landmark_points,rnp)

------ before visualization

Please correct me if I am wrong. For people who want to visualize the points in open3D, this might be helpful for you.