Plotting 3d orientation and position data (3d lines)

Hello to everyone,

I have calculated orientation and traslation data with an inertial sensor. The fact is that I want to create a 3d trayectory, plotting the gathered orientation (roll, pitch, yaw) and traslation of each iteration.

I have saved all the orientation data in a matrix of "m" x 3 size, where "m" is the number of the samples of the measurement over the time:

m = length(time);

So, my "Orientation" matrix will be something like:

orientation = zeros(length(time), 3);
for i=1:length(time)
    orientation(i,:) = [EulerAngles_Roll(i) EulerAngles_Pitch(i) EulerAngles_Yaw(i)];
end

where EulerAngles_Roll, EulerAngles_Pitch and EulerAngles_Yaw contain all the angles of roll, pitch and yaw of each iteration. Then, after some functions and some calculations, I have calculated all the points in XYZ coordinates and the vector or the line that goes from the previous point to the current point. Hence, all the points are saved in a matrix of the same size as "orientation" matrix. This matrix is called "points". It happens the same with the matrix that contains all the distances or vectors from the previous point to the current one. In order to do that, I have calculated the directional unitary vector (called "vector_direc") from the orientation and then, I have multiplied thar directonal vector (3x1 vector) by the magnitude of the distance (scalar value called "ProbeAdvance_Iter") of each iteration, obtaining "step" . So:

Starting_Eul_Orient = [EulerAngles_Roll(1) EulerAngles_Pitch(1) EulerAngles_Yaw(1)];
Starting_point = [0 0 0];
orientation(1,:) = Starting_Eul_Orient;
point(1,:) = [0 0 0];
Starting_Vector_Direc = Calc_Vector_Directional(Starting_Eul_Orient(1), Starting_Eul_Orient(2), Starting_Eul_Orient(3));%calcular vector direccional
vector_direc(1,:) = Starting_Vector_Direc;

for i=2:length(time)
    orientation(i,:) = [EulerAngles_Roll(i) EulerAngles_Pitch(i) EulerAngles_Yaw(i)];
    vector_direc(i,:)= Calc_Vector_Directional(orientation(i,1), orientation(i,2), orientation(i,3));
    step(i-1,:) = vector_direc(i-1,:)*ProbeAdvance_Iter(i-1);
    point(i,:) = point(i-1,:) + step(i-1,:);
end

I know that this is a way to represent the points in 3d:

figure(9)
plot3(point(:,1),point(:,2),point(:,3),'+');

But I would like to represent not only the point, but also the orientation and at least, the lines from one point to the next one.

I would appreciate any idea. Thanks.

Answer 1

[Thomas Satterly]

Check out quiver3 - it basically plots a vector field for the supplied points. For your case, you'll want X, Y, and Z to be the positions, and U, V, and W to be the orientation and, possibly, the orientation multiplied by the velocity magnitude to better visualize the relative speed at each point. Your X, Y, and Z data will need to be in reference to an origin, not just the difference between the last position and the current point. You can use cumsum to help quickly calculate position.

Answer 2

[Aitor Burdaspar]

Hello Thomas. Firstly, thanks for your quick answer.

I found the command quiver3 before, but I think I have not understood correctly. I have calculated the absolute XYZ coordinates from the origin... but the U,V,W part of quiver3 function refers to the orientation (roll, pitch, yaw), or refers to the directional unitary vector (in my case "vector_direc")? As you have said I understand that it refers to the roll, pitch and yaw, because the graph I achieve in this way makes more sense.

Anyway, I would like to graph the points and join them through lines in order to see in a better way the route step by step. I say that because with quiver3 I get small arrows in different Cartesian positions, and there are sections where I don´t know where it is going, since there are many points very close to each other.

I have seen that the command "line" can do something like that... but I don´t achieve it. I will continue trying. Anyway, thank you Thomas! :)

[Thomas Satterly]

Your orientation should be Cartesian, not Euler angles. And yes, plotting a line (either directly with “line” as you discovered, or “plot3” as a more general approach) represents the trajectory. Try subsampling the data for quiver plots so you don’t have so much going on, that should clear it up a bit.

Alternatively, if you want to visualize the true orientation a bit better (such as roll angle, which is difficult to communicate with a vector), you can create multiple patch objects at moments in time and the rotate command to orient them.

[Aitor Burdaspar]

Ok, Thomas. Thank you very much. When you say "Cartesian orientation", what do you mean exactly: the direction vector v = (u,v,w)? In my case, "vector_direc" or the "step"?

Have a good day,

Answer 3

[Aitor Burdaspar]

I mean... I have achieved to plot the route in 3D with a line in the following way with the command line:

x = point(:,1);
y = point(:,2);
z = point(:,3);
line(x,y,z)
view(3)

but I would like to plot not only the path, also the points and the orientations of them. So, I think I have to combine the commands you say with this one.