Plot Camera Trajectory

Question

Given a set of 4x4 pose matrices, one can derive the camera's euclidean coordinate system location as the following:

where R is the 3x3 rotation matrix and t is the translation vector of the pose, as per this question.

When the set of poses is treated in a sequential manner, such as when each refers to a camera's pose at some time step, the rotation and translation components can be accumulated as follows:

and

Where both can be plugged in to the first equation to yield the camera's relative position at a given time step.

My question is how to plot such points using OpenCV or a similar tool. For a camera moving around an object in a circular motion, the output plot should be circular, with the origin at the starting point of the trajectory.

An example is shown below:-

Though my question is not explicitly about plotting the axes as shown above, it would be a bonus.

TL;DR: Given a set of poses, how can we generate a plot like the one above with common tools such as OpenCV, VTK, Matplotlib, MATLAB etc.

Can you use Blender too? An animation is often a more useful visualization of the motion. — Francesco Callari, Mar 07 '17 at 18:58
@FrancescoCallari If it could give me the desired output then it is an option. However, I have never used Blender. — Jack H, Mar 07 '17 at 19:47

score 2 · Accepted Answer · edited May 23 '17 at 12:25

obtain axises vectors X,Y,Z and position O for each plot point

simply extract them form matrix. See Understanding 4x4 homogenous transform matrices. Now I do not know if your matrices are already inverse or not. So if your matrices represent camera coordinate system (not inverted) extract needed info directly. If not first invert the matrix and then extract.

If you got homogenuous transform matrix then you can do pseudo inverse by exploiting transpose operation. For more info see full pseudo inverse matrix.
Render each plot point

so first plot the axises as lines:
```
  red_line(O,O+a*X);
green_line(O,O+a*Y);
 blue_line(O,O+a*Z);
```
where a is axis lines size. And after this plot a dot for the position
```
black_circle(O,r);
```
Where r is some radius. You can use any gfx lib/engine for the plot. I would go for GDI or OpenGL but that depends solely on what are you familiar with.

BTW. to improve avarenes of the time line you can modulate the colors intensity (start with dark and end with bright colors so you see where the motion starts and ends ...)

Plot Camera Trajectory

1 Answers1