def test_calcPointCloudMetric():
pointsA = [(1,0,0), (0,0,0), (0,0,1)]
R = mmMath.exp(mmMath.v3(0,1,0), math.pi/4)
pointsB = []
for p in pointsA:
pointsB.append(np.dot(R, p)+(0,1,0))
T = mmMath.I_SE3()
distance = cmt.calcPointCloudMetric(pointsA, pointsB, T)
print distance
print T
pointsC = []
for p in pointsB:
pointsC.append(mmMath.T2p(np.dot(T, mmMath.p2T(p))))
viewer = ysv.SimpleViewer()
def extraDrawCallback():
for p in pointsA:
ygh.drawPoint(p, (255,0,0))
for p in pointsB:
ygh.drawPoint(p, (0,255,0))
for p in pointsC:
ygh.drawPoint(p, (0,255,255))
viewer.setExtraDrawCallback(extraDrawCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def update(self, posture):
if self.rootFixed:
T = mm.I_SE3()
joint = posture.skeleton.root
self._updateBody(joint, T, posture)
else:
OdeModel.update(self, posture)
def __init__(self, default_color = (0,255,0)):
self.pointMap = {}
self.vectorMap = {}
self.vectorOriginMap = {}
self.SO3Map = {}
self.SO3OriginMap = {}
self.colorMap = {}
self.coordinate = mmMath.I_SE3()
self.point_size = 3.0
self.line_width = 1.0
self.default_color = default_color
def test_primitives_renderers():
boxSize = [1, 1, 1]
boxFrame = mm.I_SE3()
cylinderSize = [1, 1]
cylinderFrame = mm.I_SE3()
sphereRadius = 1.
spherePosition = (0, 0.5, 0)
arrowVector = (1, 0, 0)
arrowPoint = (0, 0, 0)
viewer = ysv.SimpleViewer()
# viewer.doc.addRenderer('box', BoxesRenderer([boxSize], [boxFrame], (255,0,0)))
# viewer.doc.addRenderer('cylinder', CylindersRenderer([cylinderSize], [cylinderFrame], (0,0,255)))
# viewer.doc.addRenderer('sphere', SpheresRenderer([sphereRadius], [spherePosition], (0,0,255)))
viewer.doc.addRenderer(
'sphere',
WideArrowRenderer([arrowVector], [arrowPoint], (255, 0, 0), 1.,
.1))
viewer.show()
Fl.run()
def test_FramesRenderer_OrientationsRenderer():
frame0 = mm.I_SE3()
frame1 = mm.Rp2T(mm.exp(mm.v3(0, 1, 0), math.pi / 8.), (1, 0, 0))
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('frame0', FramesRenderer([frame0], (255, 0, 0)))
viewer.doc.addRenderer('frame1', FramesRenderer([frame1], (255, 0, 0)))
viewer.doc.addRenderer(
'orientation0',
OrientationsRenderer([mm.T2R(frame0)], [mm.T2p(frame0)],
(0, 255, 0)))
viewer.doc.addRenderer(
'orientation1',
OrientationsRenderer([mm.T2R(frame1)], [mm.T2p(frame1)],
(0, 255, 0)))
viewer.show()
Fl.run()