首页
登录 | 注册

计算机视觉基础---画出三维空间运动轨迹以及位姿

最近项目遇到路径规划的工作,每次出来路径苦于没有直观的表示方法,于是重新温习了matlab自己编写了这么一个函数,来实现 对物体三维运动过程中的展示。

已有:物体在轨迹一些点的 三维坐标和姿态(这里用欧拉角表示的);

效果:在matlab三维绘图中 画出三维点的位置和 物体处于该位置处的姿态;

pathPoints.m

pick=[285.601624 273.480804 216.159119 180.000000 0.010000 0.010000 ;218.934967 206.814148 196.159119 180.000000 0.010000 0.010000 ;218.934967 206.814148 196.159119 180.000000 0.010000 0.010000 ;152.268295 140.147491 196.159119 180.000000 -36.148727 -134.223557 ;102.897903 89.420563 99.260513 180.000000 -36.148727 -134.223557 ;53.527515 38.693626 2.361897 180.000000 -36.148727 -134.223557]
put=[135.811508 123.238525 163.859573 180.000000 -36.148727 -134.223557 ;135.811508 123.238525 163.859573 180.000000 -36.148727 -134.223557 ;411.480774 346.804260 317.149994 180.000000 0.010000 0.010000 ;411.480774 346.804260 317.149994 180.000000 0.010000 0.010000 ;687.150024 570.369995 287.149994 154.039993 -88.059998 25.230000 ;687.150024 570.369995 117.150002 154.039993 -88.059998 25.230000]
a=importdata("D:\pathPoints.m")
temp=a(2,1)     %cell
temp1=char(temp)    %cell to char
n1=strfind(temp1,'[')
n2=strfind(temp1,']')
new=temp1(n1+1:n2-1)
new1=strsplit(new,';')
fid=fopen('d:\put.m','wt')
for i=1:6
    t=strsplit(char(new1(1,i)),' ')
    for j=1:6
        value=t(1,j);
        fprintf(fid,'%s ',char(value))
    end
    fprintf(fid,'\n')
end
A=load("d:\put.m");
x=A(:,1)
y=A(:,2)
z=A(:,3)
plot3(x,y,z,'b.','MarkerSize',15)    
hold on;
no_text={"1","2","3","4","5","6"}
text(x+5,y+5,z+5,no_text)
u=A(:,4)
v=A(:,5)
w=A(:,6)
n=size(u)
n=n(1)  %number of rows
%transform the pose of vector 0,0,1 using eul in W-coordinate
for i=1:n
    R=euler2R(u(i,1),v(i,1),w(i,1),'XYZ')
    oz=[0 0 1]'
    oz= R*oz
    B(i,:)=oz'
end
U=B(:,1)
V=B(:,2)
W=B(:,3)
quiver3(x,y,z,U,V,W,0.2,'r');


最终效果,这样计算不用上机调试 也可以对规划出来的路径有个大概的了解啦! 姿态对不对就一目了然了

计算机视觉基础---画出三维空间运动轨迹以及位姿

计算机视觉基础---画出三维空间运动轨迹以及位姿

既然一个方向在变换后可以确定,那么三个坐标轴方向也都可以确定,进一步将运动物体所在的局部坐标系确定下来;

a=importdata("pathPoints.m")
temp=a(2,1)     %cell
temp1=char(temp)    %cell to char
n1=strfind(temp1,'[')
n2=strfind(temp1,']')
new=temp1(n1+1:n2-1)
new1=strsplit(new,';')
fid=fopen('d:\put.m','wt')
for i=1:6
    t=strsplit(char(new1(1,i)),' ')
    for j=1:6
        value=t(1,j);
        fprintf(fid,'%s ',char(value))
    end
    fprintf(fid,'\n')
end
A=load("d:\put.m");
x=A(:,1)
y=A(:,2)
z=A(:,3)
plot3(x,y,z,'b.','MarkerSize',15)    
hold on;
no_text={"1","2","3","4","5","6"}
text(x+5,y+5,z+5,no_text)
u=A(:,4)
v=A(:,5)
w=A(:,6)
n=size(u)
n=n(1)  %number of rows
%transform the pose of vector 0,0,1 using eul in W-coordinate
for i=1:n
    R=euler2R(u(i,1),v(i,1),w(i,1),'XYZ')
    oi=[1 0 0;0 1 0;0 0 1]
    oi= R*oi
    B=oi'
    Ux(i,1)=B(1,1)
    Vx(i,1)=B(1,2)
    Wx(i,1)=B(1,3)
    
    Uy(i,1)=B(2,1)
    Vy(i,1)=B(2,2)
    Wy(i,1)=B(2,3)
    
    Uz(i,1)=B(3,1)
    Vz(i,1)=B(3,2)
    Wz(i,1)=B(3,3)

end
[x(n+1,1),y(n+1,1),z(n+1,1)]=deal(0,0,0)
[Ux(n+1,1),Vx(n+1,1),Wx(n+1,1),Uy(n+1,1),Vy(n+1,1),Wy(n+1,1),Uz(n+1,1),Vz(n+1,1),Wz(n+1,1)]=deal(1,0,0,0,1,0,0,0,1)
text_x={"X","X","X","X","X","X","X"}
text_y={"Y","Y","Y","Y","Y","Y","Y"}
text_z={"Z","Z","Z","Z","Z","Z","Z"}
quiver3(x,y,z,Ux,Vx,Wx,0.2,'r');
text(x+30*Ux,y+30*Vx,z+30*Wx,text_x)
quiver3(x,y,z,Uy,Vy,Wy,0.2,'r');
text(x+30*Uy,y+30*Vy,z+30*Wy,text_y)
quiver3(x,y,z,Uz,Vz,Wz,0.2,'r');
text(x+30*Uz,y+30*Vz,z+30*Wz,text_z)
hold off

一目了然! 

计算机视觉基础---画出三维空间运动轨迹以及位姿



2020 jeepxie.net webmaster#jeepxie.net
10 q. 0.008 s.
京ICP备10005923号