用matlab 绘制osg平台得到路径轨迹

来源：互联网发布：数据分析相关岗位编辑：程序博客网时间：2024/06/05 18:00

osg平台得到的路径是一帧一帧的，而且还包含头文件，以及绘制坐标时用不到的分组信息等。所以要提前把得到的路径信息做预处理，这里自己写了一个预处理程序，能够提取出路径的坐标。下面贴出程序，供有缘人使用。下面这段Ｃ＃程序，把ｏｓｇ平台的路径坐标处理好了。

using System;using System.Collections.Generic;using System.Linq;using System.Text;using System.Threading.Tasks;using System.IO;namespace ConsoleApplication1{    class Program    {        static void Main(string[] args)        {            var file = File.Open("D:\\3door.txt", FileMode.Open);            List<string> txt = new List<string>();            using (var stream = new StreamReader(file))            {                while (!stream.EndOfStream)                {                    txt.Add(stream.ReadLine());                }                Console.WriteLine(txt.Count);                int temp = 0;                int index = 0;                FileStream fcreat, fwrite;                for (int i = 0; i < txt.Count; i++)                {                    if (txt[i] == "tms"||txt[i]=="")                    {                        Console.WriteLine(txt[i]);                        index = 0;                        continue;                    }                    else                    {                        index++;                                                                            fwrite = new FileStream("D:\\path\\" + "0" + temp + ".txt", FileMode.Append, FileAccess.Write);                            txt[i] = txt[i].Replace("", " ");                            string temptext = txt[i].Replace("", " ");                            string[] tempStr = temptext.Split(' ');                                                      byte[] str3 = System.Text.Encoding.Default.GetBytes(tempStr[3]);                            byte[] str5 = System.Text.Encoding.Default.GetBytes(tempStr[5]);                            string nextine = "\r\n";                            string s = " ";                            byte[] empty = System.Text.Encoding.Default.GetBytes(s);                            byte[] nextlines = System.Text.Encoding.Default.GetBytes(nextine);                            fwrite.Write(str3, 0, str3.Length);                            fwrite.Write(empty, 0, empty.Length);                            fwrite.Write(str5, 0, str5.Length);                            fwrite.Write(nextlines, 0, nextlines.Length);                            fwrite.Flush();                            fwrite.Close();                                              }                }            }        }    }}

处理后的路径和处理前的路径对比图：

处理好路径后，就可以用MATLAB 进行绘制了，这里也给出matlab程序，

for frames = 1:1:1szBuffer = sprintf('3door.txt'); f1=fopen(szBuffer);cla;i=1;frame=1;y=1;while 1    nextline = fgetl(f1); %读第一行     if ~isstr(nextline), break, end %读到最后跳出     % disp(nextline);%这行可以不要     if mod(i,161)==0        frame=frame+1;        i=1;    end    a = sscanf(nextline, '%f %f');    disp(a);    datax(frame,i)=a(1);    datay(frame,i)=a(2);    i=i+1;endfclose(f1);for num = 1:1:160    x=datax(:,num);    y=datay(:,num);    plot(x,y,'r-');    hold on;end%for wall---------------  LEFT = -10.0;  RIGHT = 310.0;  TOP = 210.0;  BOTTOM = -10.0;%------------------------- %障碍物文件inBuffer = sprintf('obstacle/door_5obs.txt');%打开障碍物文件f2=fopen(inBuffer);i=1;nextline = fgetl(f2)%读第一行 if ~isstr(nextline), break, end %读到最后跳出     disp(nextline);%这行可以不要 a = sscanf(nextline, '%d');%读取数据，根据你自己的需要改 for num=1:1:a    nextline = fgetl(f2); %读第一行     if ~isstr(nextline), break, end %读到最后跳出     b = sscanf(nextline, '%f %f');    data_obj(i)   = b(1);    data_obj(i+1) = b(2);    i = i+2;endnextline = fgetl(f2); %读第一行 a = sscanf(nextline, '%d');%读取数据，根据你自己的需要改。障碍物个数for num=1:1:a %    nextline = fgetl(f2); %读第一行     if ~isstr(nextline), break, end %读到最后跳出     vertnum = sscanf(nextline, '%d ');%障碍物顶点个数        %读取顶点坐标，并画出障碍物    for i=2:1:vertnum(1)+1        b(1) = vertnum(i)+1;        t = i+1;        if t> vertnum(1)+1            t = 2;        end        b(2) = vertnum(t)+1;        plot([data_obj(2*b(1)-1) data_obj(2*b(2)-1)],[data_obj(2*b(1)) data_obj(2*b(2))]);        set(gcf,'Position',[500 200 500 500])        set(gca,'Position',[.13 .17 .80 .74]);        axis([LEFT RIGHT BOTTOM TOP]);         hold on;    end   endfclose(f2);szBuffer = sprintf('./jepg_emotion/%02d.jpeg',frame);print(gcf,'-djpeg',szBuffer);end

给出绘制的最终结果：

这就是最终的描绘结果，当然上述程序中，所有的路径文件需要根据自己的需求来改。

1 0