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

最近项目遇到路径规划的工作，每次出来路径苦于没有直观的表示方法，于是重新温习了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