def __init__(self, feas, width, height):
self.feas = feas
# 创建模型占用地图
self.map = Map2D(width, height)
# 默认角度设定为0
self.angle = 0
self.__initFeasOnMap()
def __TestModelDrawFunc():
# 创建一个大地图
map = Map2D(1500, 1000)
# 引入模型类以及模型
from map.objModel import ObjModel
from map.objModel import Models
# 获取汽车模型
mdl = ObjModel(Models.carModel, 170, 375)
# 创建两个角度不同的汽车
m_car1 = mdl.getRotateModel(103)
m_car2 = mdl.getRotateModel(87)
# 将车加入地图中
map.addSubMap(m_car1, 0, 0)
map.addSubMap(m_car2, 597, -20)
# 创建画板
fig = MapFigure(map)
# 使用栅格绘制模型图
fig.drawMapByGrids() # This one is slow
fig.setXYLim(0, 1500, 0, 1000)
fig.setAspect()
# 获取传感器数据
from sensor.radar2D import Radar2D
radar = Radar2D([0, math.pi], 510, 800, 1, 5)
radar_pos = [600, 200, 0]
radar_data = radar.getMeasureData(map, radar_pos)
# 解析数据
xs = [x[0] for x in radar_data]
ys = [x[1] for x in radar_data]
# 画在图上
for dat in radar_data:
print dat[0], dat[1]
fig.drawRadarPoints(xs, ys, 'r.')
# 展示模型图
fig.showFigure()
def __TestBasicDrawFunc():
# 首先创建一个200x200地图
map = Map2D(200, 200)
# 地图中添加一个矩形
map.addFeature(Map2D.Feature(Map2D.Feature.rectangle, [50, 50, 10, 20]))
# 创建一个50x50的另一个地图
map2 = Map2D(50, 50)
# 添加椭圆特征
map2.addFeature(Map2D.Feature(Map2D.Feature.ellipse, [25, 25, 40, 30, 0]))
# map融合map2，把map2放到0，0位置
map.addSubMap(map2, 0, 0)
# 以下是画图部分
fig = MapFigure(map)
fig.drawMapByGrids() # This one is slow
fig.setXYLim(0, 200, 0, 200)
fig.showFigure()
def drawSensorLocErr():
fig = MapFigure(Map2D(200, 200))
l1 = fig.drawErrorBar(x, yLaser, eLaser, 'm-s')
l2 = fig.drawErrorBar(x, ySonic, eSonic, 'b--o')
fig.setXYLabel(u'栅格单元/cm', u'平均定位误差/cm')
fig.setXYLim(0, 70, 0, 35)
plt.legend([l1, l2], [u'LaserRad', u'SonicSen'], loc='upper right')
fig.showFigure()
def drawPNum():
fig = MapFigure(Map2D(200, 200))
l1 = fig.drawRadarPoints(n_x, n_fix1, 'm-s')
l2 = fig.drawRadarPoints(n_x, n_fix2, 'b--*')
l3 = fig.drawRadarPoints(n_x, n_ada, 'g:o', 1.5)
plt.legend([l1, l2, l3], [u'FIX1', u'FIX2', 'ADA'], loc='upper right')
fig.setXYLabel(u'数据编号', u'粒子数/个')
fig.showFigure()
def sim1(init_val):
num = 60
sam = 60
seqX = PFSim.getSeqX(init_val, num)
seqO = PFSim.getSeqO(seqX)
rans = PFSim.genNormalRan(num, sam)
# SIS
seqR = [init_val]
timeSIS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingSIS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqR.append(pset.getCurrentVal())
timeSIS = time.clock() - timeSIS
# LVS
seqLVS = [init_val]
timeLVS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingLVS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqLVS.append(pset.getCurrentVal())
timeLVS = time.clock() - timeLVS
# VIS
seqVIS = [init_val]
timeVIS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingVIS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqVIS.append(pset.getCurrentVal())
timeVIS = time.clock() - timeVIS
fig = MapFigure(Map2D(200, 200))
index = np.arange(0, num, 1)
print len(index), len(seqX)
print PFSim.getRMSE(seqX, seqR)
print PFSim.getRMSE(seqX, seqLVS)
print PFSim.getRMSE(seqX, seqVIS)
print timeSIS, timeLVS, timeVIS
l1 = fig.drawRadarPoints(index, seqX, 'ro-')
l2 = fig.drawRadarPoints(index, seqR, 'm<:', 1.5)
l3 = fig.drawRadarPoints(index, seqLVS, 'c<:', 1.5)
l4 = fig.drawRadarPoints(index, seqVIS, 'g*--', 1.5)
plt.legend([l1, l2, l3, l4], [u'True', u'SIS', 'LVS', 'VIS'],
loc='upper right')
fig.setXYLabel(u'数据编号', u'距离/mm')
#print seqR
fig.showFigure()
def __TestRadarDrawFunc():
# 创建一个大地图
map = Map2D(2000, 2000)
# 添加两个矩形障碍物
map.addFeature(
Map2D.Feature(Map2D.Feature.rectangle, [100, 500, 500, 1400]))
map.addFeature(
Map2D.Feature(Map2D.Feature.rectangle, [1400, 600, 600, 1200]))
# 创建雷达类&&获取位置点
radar = Radar2D([0, math.pi], 510, 5600, 1, 5)
radar_pos = radar.getMeasureData(map, [1000, 0, 0])
# 绘制地图
fig = MapFigure(map)
fig.drawMapByFeatures()
fig.drawRadarPoints([x[0] for x in radar_pos], [x[1] for x in radar_pos],
'r.')
fig.setXYLim(0, 2000, 0, 2000)
fig.showFigure()
def drawParticleSize():
y = []
for i in range(len(x)):
y.append(PFSim.genKLDNum(6400 / x[i]))
fig = MapFigure(Map2D(200, 200))
l = fig.drawRadarPoints(x, y, 'bo-')
fig.setXYLabel(u'栅格单元/cm', u'平均采样数/个')
#plt.legend([l], [u'KLD'], loc = 'upper right')
fig.showFigure()
def getRotateModel(self, angle):
w, h = self.map.m_width, self.map.m_height
#print w, h
l = max(w, h) + 100
l2 = 2 * l
n_map = Map2D(l2, l2)
for i in range(0, w):
for j in range(0, h):
dis = math.sqrt(i**2 + j**2)
ang = 0
if dis != 0:
ang = math.acos(i / dis) + angle / 180.0 * math.pi
nx = dis * math.cos(ang)
ny = dis * math.sin(ang)
#print nx, ny
n_map.setValInMap(
l + nx, l + ny,
self.map.s_grid[i][j]) #self.map.s_grid[i][j]
return n_map
def sim2(init_val):
num = 20
sam = 60
seqX = PFSim.getSeqX(init_val, num)
seqO = PFSim.getSeqO(seqX)
rans = PFSim.genNormalRan(num, sam)
# SIS-KLD-FIX
seqFIX1 = [init_val]
timeSIS = time.clock()
nums1 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
toSam = PFSim.genKLDNum(max(1, pset.getOccRoom() / 80))
nums1.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqFIX1.append(pset.getCurrentVal())
timeSIS = time.clock() - timeSIS
# SIS-KLD-FIX
seqFIX2 = [init_val]
timeFIX2 = time.clock()
nums2 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
toSam = PFSim.genKLDNum(max(1, pset.getOccRoom() / 240))
nums2.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqFIX2.append(pset.getCurrentVal())
timeFIX2 = time.clock() - timeFIX2
# SIS-KLD-ADA
seqADA = [init_val]
timeADA = time.clock()
nums3 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
ori = pset.getOccRoom() / 200
toSam = PFSim.genKLDNum(min(7, ori))
nums3.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqADA.append(pset.getCurrentVal())
timeADA = time.clock() - timeADA
fig = MapFigure(Map2D(200, 200))
index = np.arange(0, num, 1)
print len(index), len(seqX)
print nums1
print nums2
print nums3
print timeSIS, PFSim.getRMSE(seqX, seqFIX1)
print timeFIX2, PFSim.getRMSE(seqX, seqFIX2)
print timeADA, PFSim.getRMSE(seqX, seqADA)
l1 = fig.drawRadarPoints(index, seqX, 'ro-')
l2 = fig.drawRadarPoints(index, seqFIX1, 'm<:', 1.5)
l3 = fig.drawRadarPoints(index, seqFIX2, 'bd:', 1.5)
l4 = fig.drawRadarPoints(index, seqADA, 'g*--', 1.5)
plt.legend([l1, l2, l3, l4], [u'True', u'FIX1', 'FIX2', 'ADA'],
loc='upper right')
fig.setXYLabel(u'数据编号', u'距离/mm')
#print seqR
fig.showFigure()
class Models:
carModel = [
Map2D.Feature(Map2D.Feature.ellipse, [20, 20, 40, 40, 0]), # 车尾
Map2D.Feature(Map2D.Feature.rectangle, [20, 0, 130, 20, 0]),
Map2D.Feature(Map2D.Feature.ellipse, [150, 20, 40, 40, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [0, 20, 170, 25, 0]),
Map2D.Feature(Map2D.Feature.ellipse, [45, 338, 90, 74, 0]), #车头
Map2D.Feature(Map2D.Feature.ellipse, [125, 338, 90, 74, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [45, 338, 80, 37, 0]),
Map2D.Feature(Map2D.Feature.erase, [0, 280, 170, 58, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [0, 320, 170, 18, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [0, 85, 20, 195, 0]), # 中间
Map2D.Feature(Map2D.Feature.rectangle, [150, 85, 20, 195, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [10, 45, 10, 40, 0]), #车轮
Map2D.Feature(Map2D.Feature.rectangle, [150, 45, 10, 40, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [20, 65, 130, 4, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [10, 280, 10, 40, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [150, 280, 10, 40, 0]),
Map2D.Feature(Map2D.Feature.rectangle, [20, 300, 130, 4, 0]),
]
if radar_data[bps[len(bps) - 1]][2] != 0:
ret_range.append([bps[len(bps) - 1][2], len(radar_data)])
return ret_range
# test break point method
if __name__ == '__main__':
# import files
import sys
sys.path.append('..')
from figure.mapFigure import MapFigure
from map.map2D import Map2D
from sensor.radar2D import Radar2D
from split import Split
#
map = Map2D(2000, 2000)
map.addFeature(
Map2D.Feature(Map2D.Feature.rectangle, [100, 500, 500, 1400]))
map.addFeature(
Map2D.Feature(Map2D.Feature.rectangle, [1400, 600, 600, 1200]))
radar = Radar2D([0, math.pi], 510, 5600, 1, 5)
radar_pos = [1000, 0, 0]
radar_data = radar.getMeasureData(map, radar_pos)
bps = DBSCAN.getBreakPoints(radar_data, radar_pos)
rag = DBSCAN.getFeatureRange(radar_data, radar_pos)
for i in range(len(radar_data)):
print i, radar_data[i]
print bps
print rag
def getBasicMap(width=3000, height=3000, feature=Feas.fea1):
map = Map2D(width, height)
for i in range(len(feature)):
map.addFeature(Map2D.Feature(Map2D.Feature.rectangle, feature[i]))
return map