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.
$\begingroup$
$\endgroup$
Add a comment
|
1 Answer
$\begingroup$
$\endgroup$
4
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.
-
-
$\begingroup$ @rotating_image each colum of R is composed of [rx ry rz]^T. $\endgroup$ Commented Oct 1, 2019 at 5:07
-
1$\begingroup$ 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 ? $\endgroup$ Commented Oct 1, 2019 at 15:43
-
$\begingroup$ @rotating_image that's right. please don't forget to upvote or select as the answer if it was helpful. $\endgroup$ Commented Oct 3, 2019 at 1:31