Python make_set示例

示例#1

文件： test.py 项目： gt-ros-pkg/hrl

#Others
import nodes as nd
import transforms2d as t2d
import numpy as np
import math as mt
import opencv.cv as cv
import types
import functools as ft

velocity     = np.matrix([0.01, 0.0]).T
pose         = t2d.Pose2D(0.0, 0.0, 0.0)

#Setup
mvar         = rm.motion_var()
particles    = rm.make_set(mvar, pose, 100)

#Run
motion_model = rm.RobotMotion(mvar)
app_model    = da.DetectionAppearance(cov=np.matrix([[(.03*.03), 0], [0, (.03*.03)]]))
filter       = pf.PFilter(motion_model, app_model)
display      = nd.RobotDisp("particle filter", size = 2, draw_center=True, meters_radius=10)

max_weight   = app_model.weight(np.matrix([1.0, 0.0]).T, t2d.Pose2D(1.0, 0.0, 0.0))
draw_func    = ft.partial(rm.draw_weighted_Pose2D, display, max_weight)
cur_pos      = pose.pos.copy()
cur_set      = particles

for i in xrange(100):
    display.clear()
    cur_set = filter.step(t2d.Pose2D(velocity[0,0], velocity[1,0], 0), 

示例#2

文件： test.py 项目： wklharry/hrl

#Others
import nodes as nd
import transforms2d as t2d
import numpy as np
import math as mt
import opencv.cv as cv
import types
import functools as ft

velocity = np.matrix([0.01, 0.0]).T
pose = t2d.Pose2D(0.0, 0.0, 0.0)

#Setup
mvar = rm.motion_var()
particles = rm.make_set(mvar, pose, 100)

#Run
motion_model = rm.RobotMotion(mvar)
app_model = da.DetectionAppearance(
    cov=np.matrix([[(.03 * .03), 0], [0, (.03 * .03)]]))
filter = pf.PFilter(motion_model, app_model)
display = nd.RobotDisp("particle filter",
                       size=2,
                       draw_center=True,
                       meters_radius=10)

max_weight = app_model.weight(
    np.matrix([1.0, 0.0]).T, t2d.Pose2D(1.0, 0.0, 0.0))
draw_func = ft.partial(rm.draw_weighted_Pose2D, display, max_weight)
cur_pos = pose.pos.copy()

示例#3

文件： test_object_motion.py 项目： wklharry/hrl

import nodes as nd
import transforms2d as t2d
import numpy as np
import math as mt
import opencv.cv as cv
import types
import functools as ft

velocity     = np.matrix([1.0, 0.0]).T
pose         = t2d.Pose2D(2.0, 0.0, 0.0)

#Setup
mvar         = rm.motion_var()
def to_pos(p):
    return p.pos
particles    = map(to_pos, rm.make_set(mvar, pose, 100))

#Run
robot_motion = rm.RobotMotion(mvar)
motion_model = om.ObjectMotion(robot_motion)
app_model    = da.DetectionAppearance(cov=np.matrix([[(.03*.03), 0], [0, (.03*.03)]]))

filter       = pf.PFilter(motion_model, app_model)
display      = nd.RobotDisp("particle filter", size = 2, draw_center=True, meters_radius=10)

max_weight   = app_model.weight(np.matrix([1.0, 0.0]).T, np.matrix([1.0, 0.0]).T)
draw_func    = ft.partial(da.draw_weighted_2D, display, max_weight)
cur_pos      = pose.pos.copy()
cur_set      = particles

while True: