osg平台得到的路径是一帧一帧的,而且还包含头文件,以及绘制坐标时用不到的分组信息等。所以要提前把得到的路径信息做预处理,这里自己写了一个预处理程序,能够提取出路径的坐标。下面贴出程序,供有缘人使用。下面这段C#程序,把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
给出绘制的最终结果:
这就是最终的描绘结果,当然上述程序中,所有的路径文件需要根据自己的需求来改。