# How to plot the 3D camera trajectory from a VSLAM output

I am totally new to VSLAM algorithms and would want to plot the path followed by the moving camera. I am using the open source implementation of ORB_SLAM2 for a rgb-d camera on benchmark dataset. The output of the algorithm produces a CameraTrajectory.txt file. The file is in the format : [time_stamp] [tx] [ty] [tz] [qx] [qy] [qz] [qw] . I am aware of that the rotations are in Quarternion form but cannot figure out how to plot the full 3D plot of the camera path from these seven values tx,ty,tz,qx,qy,qz,qw . I need a [time_stamp] [x] [y] [z] format.

## 1 Answer

That's simple.

If you use matlab or opengl what you need to do is just drawing 3 axis at (tx,ty,tz). You need to convert quaternion to rotation matrix.

(qx,qy,qz,qw) -> R(3x3 matrix)

where each col of R is the axis you need. The axis just a line segment where it start from (tx,ty,tz) and end at (tx+rx,ty+ry,tz+rz).

There are libraries for quaternion to rotation conversion. You don't need to implement it yourself.

• how do i get rx ry rz? – rotating_image Sep 30 '19 at 14:35
• @rotating_image each colum of R is composed of [rx ry rz]^T. – C.O Park Oct 1 '19 at 5:07
• what if I just want the 3D trajectory of the camera centre? if I assume the starting point of the camera centre as X0 = (0,0,0)^T and I follow X(t) = R(t)*X0 + T(t) ...then basically I am plotting T(t) . Is this method correct ? – rotating_image Oct 1 '19 at 15:43
• @rotating_image that's right. please don't forget to upvote or select as the answer if it was helpful. – C.O Park Oct 3 '19 at 1:31