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

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:1
szBuffer = 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;
end
fclose(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;
end
nextline = 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   
end
fclose(f2);
szBuffer = sprintf('./jepg_emotion/%02d.jpeg',frame);
print(gcf,'-djpeg',szBuffer);
end

给出绘制的最终结果：

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