def processSnippet():
obj = om.getOrCreateContainer("continuous")
om.getOrCreateContainer("cont debug", obj)
if continuouswalkingDemo.processContinuousStereo:
polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet_stereo.vtp"))
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet.vtp"))
vis.updatePolyData(polyData, "walking snapshot trimmed", parent="continuous")
standingFootName = "l_foot"
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData)
if clusters is None:
print "No cluster found, stop walking now!"
return
# Step 3: find the corners of the minimum bounding rectangles
blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def fitObjectToPointcloud(self,
objectName,
pointCloud=None,
downsampleObject=True,
objectPolyData=None,
filename=None,
visualize=True,
algorithm="GoICP"):
if objectPolyData is None:
if filename is None:
filename = utils.getObjectMeshFilename(objectName)
objectPolyData = ioUtils.readPolyData(filename)
# downsample the object poly data
objectPolyDataForAlgorithm = objectPolyData
if downsampleObject:
objectPolyDataForAlgorithm = segmentation.applyVoxelGrid(
objectPolyData, leafSize=0.002)
if pointCloud is None:
pointCloud = om.findObjectByName('reconstruction').polyData
sceneToModelTransform = GlobalRegistration.runRegistration(
algorithm, pointCloud, objectPolyDataForAlgorithm)
objectToWorld = sceneToModelTransform.GetLinearInverse()
self.objectToWorldTransform[objectName] = objectToWorld
if visualize:
alignedObj = vis.updatePolyData(objectPolyData,
objectName + ' aligned')
alignedObj.actor.SetUserTransform(objectToWorld)
return objectToWorld
def testAlign(modelName, sceneName):
removeFile('model_features.bin')
removeFile('scene_features.bin')
removeFile('features.bin')
for objType, objName in [('model', modelName), ('scene', sceneName)]:
print 'process', objName
pd = om.findObjectByName(objName).polyData
pd = segmentation.applyVoxelGrid(pd, leafSize=0.005)
print 'compute normals...'
pd = segmentation.normalEstimation(pd,
searchRadius=0.05,
useVoxelGrid=False,
voxelGridLeafSize=0.01)
print 'compute features...'
computePointFeatureHistograms(pd, searchRadius=0.10)
renameFeaturesFile(objType + '_features.bin')
print 'run registration...'
subprocess.check_call(['bash', 'runFGR.sh'])
showTransformedData(sceneName)
print 'done.'
def processSnippet():
obj = om.getOrCreateContainer('continuous')
om.getOrCreateContainer('cont debug', obj)
if (continuouswalkingDemo.processContinuousStereo):
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp'))
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
else:
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp'))
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous')
standingFootName = cwdemo.ikPlanner.leftFootLink
standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False)
# Step 2: find all the surfaces in front of the robot (about 0.75sec)
clusters = segmentation.findHorizontalSurfaces(polyData)
if (clusters is None):
print("No cluster found, stop walking now!")
return
# Step 3: find the corners of the minimum bounding rectangles
blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)
footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)
cwdemo.drawFittedSteps(footsteps)
def computeFeatures(polyData, name):
pd = segmentation.applyVoxelGrid(pd, leafSize=0.005)
print 'compute normals...'
pd = segmentation.normalEstimation(pd,
searchRadius=0.05,
useVoxelGrid=False,
voxelGridLeafSize=0.01)
print 'compute features...'
FastGlobalRegistration.computePointFeatureHistograms(pd,
searchRadius=0.10)
newName = name + '_features.bin'
FastGlobalRegistration.renameFeaturesFile(name)
def test(self, algorithm="GoICP"):
"""
Runs a defaul registration test with a kuka arm mesh
:param algorithm:
:return:
"""
assert algorithm in ["GoICP", "Super4PCS"]
baseName = os.path.join(utils.getLabelFusionDataDir(),
'registration-output/robot-scene')
pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshPointcloudFile = os.path.join(baseName,
'robot_mesh_pointcloud.vtp')
pointCloud = ioUtils.readPolyData(pointCloudFile)
robotMesh = ioUtils.readPolyData(robotMeshFile)
robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile)
pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01)
# rotate the scene 90 degrees
sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0],
[0, 0, 90])
# sceneTransform = transformUtils.frameFromPositionAndRPY([-1, 0, 0], [0, 0, 0])
pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform)
print pointCloud.GetNumberOfPoints()
print robotMeshPointcloud.GetNumberOfPoints()
sceneName = 'scene pointcloud'
modelName = 'model pointcloud'
vis.showPolyData(robotMeshPointcloud, modelName)
vis.showPolyData(pointCloud, sceneName)
self.view.resetCamera()
self.view.forceRender()
sceneToModelTransform = GlobalRegistration.runRegistration(
algorithm, pointCloud, robotMeshPointcloud)
modelToSceneTransform = sceneToModelTransform.GetLinearInverse()
GlobalRegistration.showAlignedPointcloud(robotMeshPointcloud,
modelToSceneTransform,
modelName + " aligned",
color=[1, 0, 0])
def testSuperPCS4(self):
"""
Test the SuperPCS4 algorithm on some default data
:return:
"""
baseName = os.path.join(CorlUtils.getCorlDataDir(),
'registration-output/robot-scene')
pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshPointcloudFile = os.path.join(baseName,
'robot_mesh_pointcloud.vtp')
pointCloud = ioUtils.readPolyData(pointCloudFile)
robotMesh = ioUtils.readPolyData(robotMeshFile)
robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile)
# PCS4 algorithm performs very differently if you remove the origin point
# pointCloud = removeOriginPoints(pointCloud)
pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01)
sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0],
[0, 0, 90])
pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform)
# robotMeshPointcloud = shuffleAndShiftPoints(robotMeshPointcloud)
# pointCloud = shuffleAndShiftPoints(pointCloud)
print pointCloud.GetNumberOfPoints()
print robotMeshPointcloud.GetNumberOfPoints()
sceneName = 'scene pointcloud'
modelName = 'model pointcloud'
vis.showPolyData(robotMeshPointcloud, modelName)
vis.showPolyData(pointCloud, sceneName)
self.view.resetCamera()
self.view.forceRender()
sceneToModelTransform = SuperPCS4.run(pointCloud, robotMeshPointcloud)
GlobalRegistration.showAlignedPointcloud(pointCloud,
sceneToModelTransform,
sceneName + " aligned")
def testICP(self, objectName, scenePointCloud=None, applyVoxelGrid=True):
print "running ICP"
visObj = om.findObjectByName(objectName)
if scenePointCloud is None:
scenePointCloud = om.findObjectByName(
'cropped pointcloud').polyData
modelPointcloud = filterUtils.transformPolyData(
visObj.polyData, visObj.actor.GetUserTransform())
if applyVoxelGrid:
modelPointcloud = segmentation.applyVoxelGrid(modelPointcloud,
leafSize=0.0005)
sceneToModelTransform = segmentation.applyICP(scenePointCloud,
modelPointcloud)
modelToSceneTransform = sceneToModelTransform.GetLinearInverse()
concatenatedTransform = transformUtils.concatenateTransforms(
[visObj.actor.GetUserTransform(), modelToSceneTransform])
visObj.getChildFrame().copyFrame(concatenatedTransform)
print "ICP finished"
def loadCube(subdivisions=30):
d = DebugData()
dim = np.array([0.11,0.11,0.13])
center = np.array([0,0,0])
d.addCube(dim, center, subdivisions=subdivisions)
polyData = d.getPolyData()
# set vertex colors of top face to green
points = vnp.getNumpyFromVtk(polyData, 'Points')
colors = vnp.getNumpyFromVtk(polyData, 'RGB255')
maxZ = points[:,2].max()
inds = points[:,2] > (maxZ - 0.0001)
colors[inds] = [0, 255, 0]
visObj = vis.showPolyData(polyData, 'tissue_box_subdivision', colorByName='RGB255')
print "number of points = ", polyData.GetNumberOfPoints()
sampledPolyData = segmentation.applyVoxelGrid(polyData, leafSize=0.0001)
visObj2 = vis.showPolyData(sampledPolyData, 'voxel grid', color=[0,1,0])
print "voxel number of points ", sampledPolyData.GetNumberOfPoints()
return (visObj, visObj2)
baseName = os.path.join(labelfusion.utils.getLabelFusionDataDir(),
'registration-output/robot-scene')
pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp')
robotMeshPointcloudFile = os.path.join(baseName, 'robot_mesh_pointcloud.vtp')
pointCloud = ioUtils.readPolyData(pointCloudFile)
robotMesh = ioUtils.readPolyData(robotMeshFile)
robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile)
# PCS4 algorithm performs very differently if you remove the origin point
#pointCloud = removeOriginPoints(pointCloud)
pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01)
sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 90])
pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform)
#robotMeshPointcloud = shuffleAndShiftPoints(robotMeshPointcloud)
#pointCloud = shuffleAndShiftPoints(pointCloud)
print pointCloud.GetNumberOfPoints()
print robotMeshPointcloud.GetNumberOfPoints()
sceneName = 'scene pointcloud'
modelName = 'model pointcloud'
vis.showPolyData(robotMeshPointcloud, modelName)
vis.showPolyData(pointCloud, sceneName)
useVoxelGrid = False
reorientNormals = True
testNormals = False
showGlyphs = False
testPlaneSegmentation = True
if reorientNormals:
polyData = filterUtils.computeNormals(polyData)
if removeGround:
groundPoints, scenePoints = segmentation.removeGround(polyData)
vis.showPolyData(groundPoints, 'ground', visible=False)
polyData = scenePoints
if useVoxelGrid:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
if testNormals:
print 'computing normals...'
f = vtk.vtkRobustNormalEstimator()
f.SetInput(polyData)
f.SetMaxIterations(100)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print('empty search region point cloud')
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
robotMesh = ioUtils.readPolyData('robot-mesh.vtp')
t = transformUtils.frameFromPositionAndRPY([0.0, 0.0, 2.0], [90, 0, 0])
robotMesh = filterUtils.transformPolyData(robotMesh, t)
robotMesh.GetPointData().SetNormals(None)
obj = vis.showPolyData(robotMesh, 'robot mesh', visible=False)
subd = vtk.vtkLoopSubdivisionFilter()
subd.SetInput(robotMesh)
subd.SetNumberOfSubdivisions(3)
subd.Update()
subdividedMesh = subd.GetOutput()
modelPoints = segmentation.applyVoxelGrid(subdividedMesh, leafSize=0.005)
vis.showPolyData(modelPoints, 'model points')
print 'model pts:', modelPoints.GetNumberOfPoints()
pointCloud = ioUtils.readPolyData('pointcloud.vtp')
obj = vis.showPolyData(pointCloud, 'pointcloud original')
'''
t = transformUtils.frameFromPositionAndRPY([0.2, 0.3, 0.4], [10,14,15])
pointCloud = filterUtils.transformPolyData(pointCloud, t)
scenePoints = segmentation.applyVoxelGrid(pointCloud, leafSize=0.005)
vis.showPolyData(scenePoints, 'scene points')
print 'scene pts:', scenePoints.GetNumberOfPoints()
'''
testAlign('model points', 'pointcloud original')
showGlyphs = False
testPlaneSegmentation = True
if reorientNormals:
polyData = filterUtils.computeNormals(polyData)
if removeGround:
groundPoints, scenePoints = segmentation.removeGround(polyData)
vis.showPolyData(groundPoints, 'ground', visible=False)
polyData = scenePoints
if useVoxelGrid:
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
if testNormals:
print 'computing normals...'
f = vtk.vtkRobustNormalEstimator()
f.SetInput(polyData)
f.SetMaxIterations(100)
f.SetMaxEstimationError(0.01)
f.SetMaxCenterError(0.02)
f.SetComputeCurvature(True)
f.SetRadius(0.1)
f.Update()
polyData = shallowCopy(f.GetOutput())
print 'done.'
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
d.addLine(originProjectedToPlane, origin, color=[0,1,0])
d.addLine(originProjectedToEdge, origin, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
