Exemple #1
0
 def loadSDF(self, filename):
     sdf = pysdf.SDF(file=filename)
     for model in sdf.world.models:
         for link in model.links:
             if hasattr(link, "submodels"):
                 if len(link.submodels) > 0:
                     print model.name + " - This is an articulated object - SKIPPING!"
                     break
             for col in link.collisions:
                 t1 = transformUtils.getTransformFromNumpy(model.pose)
                 t2 = transformUtils.getTransformFromNumpy(link.pose)
                 t3 = transformUtils.getTransformFromNumpy(col.pose)
                 t = t1
                 t.PreMultiply()
                 t.Concatenate(t2)
                 t.PreMultiply()
                 t.Concatenate(t3)
                 p = transformUtils.poseFromTransform(t)
                 name = ""  # model.name
                 color = col.color[0:3] if col.color is not None else [
                     0.8, 0.8, 0.8
                 ]
                 if len(link.name) > 0:  # and link.name != model.name:
                     name += link.name
                 if len(col.name) > 0 and len(link.collisions) > 1:
                     name += "-" + col.name
                 if col.geometry_type == "mesh":
                     print "Mesh geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "image":
                     print "image geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "height_map":
                     print "Height map geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "plane":
                     print "Plane geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "sphere":
                     print "Sphere geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "box":
                     desc = dict(
                         classname="BoxAffordanceItem",
                         Name=name,
                         uuid=newUUID(),
                         pose=p,
                         Color=color,
                         Dimensions=map(
                             float, col.geometry_data["size"].split(" ")),
                     )
                     self.affordanceManager.newAffordanceFromDescription(
                         desc)
                 if col.geometry_type == "cylinder":
                     desc = dict(
                         classname="CylinderAffordanceItem",
                         Name=name,
                         uuid=newUUID(),
                         pose=p,
                         Color=color,
                         Radius=float(col.geometry_data["radius"]),
                         Length=float(col.geometry_data["length"]),
                     )
                     self.affordanceManager.newAffordanceFromDescription(
                         desc)
Exemple #2
0
 def loadSDF(self, filename):
     sdf = pysdf.SDF(file=filename)
     for model in sdf.world.models:
         for link in model.links:
             if hasattr(link, 'submodels'):
                 if len(link.submodels) > 0:
                     print model.name + ' - This is an articulated object - SKIPPING!'
                     break
             for col in link.collisions:
                 t1 = transformUtils.getTransformFromNumpy(model.pose)
                 t2 = transformUtils.getTransformFromNumpy(link.pose)
                 t3 = transformUtils.getTransformFromNumpy(col.pose)
                 t = t1
                 t.PreMultiply()
                 t.Concatenate(t2)
                 t.PreMultiply()
                 t.Concatenate(t3)
                 p = transformUtils.poseFromTransform(t)
                 name = model.name
                 color = col.color[0:3] if col.color is not None else [
                     0.8, 0.8, 0.8
                 ]
                 if len(link.name) > 0 and link.name != model.name:
                     name += '-' + link.name
                 if len(col.name) > 0 and len(link.collisions) > 1:
                     name += '-' + col.name
                 if col.geometry_type == 'mesh':
                     print 'Mesh geometry is unsupported - SKIPPING!'
                 if col.geometry_type == 'image':
                     print 'image geometry is unsupported - SKIPPING!'
                 if col.geometry_type == 'height_map':
                     print 'Height map geometry is unsupported - SKIPPING!'
                 if col.geometry_type == 'plane':
                     print 'Plane geometry is unsupported - SKIPPING!'
                 if col.geometry_type == 'sphere':
                     print 'Sphere geometry is unsupported - SKIPPING!'
                 if col.geometry_type == 'box':
                     desc = dict(classname='BoxAffordanceItem',
                                 Name=name,
                                 uuid=newUUID(),
                                 pose=p,
                                 Color=color,
                                 Dimensions=map(
                                     float,
                                     col.geometry_data['size'].split(' ')))
                     self.affordanceManager.newAffordanceFromDescription(
                         desc)
                 if col.geometry_type == 'cylinder':
                     desc = dict(classname='CylinderAffordanceItem',
                                 Name=name,
                                 uuid=newUUID(),
                                 pose=p,
                                 Color=color,
                                 Radius=float(col.geometry_data['radius']),
                                 Length=float(col.geometry_data['length']))
                     self.affordanceManager.newAffordanceFromDescription(
                         desc)
Exemple #3
0
 def loadSDF(self, filename):
     sdf = pysdf.SDF(file=filename)
     for model in sdf.world.models:
         for link in model.links:
             if hasattr(link, "submodels"):
                 if len(link.submodels) > 0:
                     print model.name + " - This is an articulated object - SKIPPING!"
                     break
             for col in link.collisions:
                 t1 = transformUtils.getTransformFromNumpy(model.pose)
                 t2 = transformUtils.getTransformFromNumpy(link.pose)
                 t3 = transformUtils.getTransformFromNumpy(col.pose)
                 t = t1
                 t.PreMultiply()
                 t.Concatenate(t2)
                 t.PreMultiply()
                 t.Concatenate(t3)
                 p = transformUtils.poseFromTransform(t)
                 name = model.name
                 if len(link.name) > 0 and link.name != model.name:
                     name += "-" + link.name
                 if len(col.name) > 0 and len(link.collisions) > 1:
                     name += "-" + col.name
                 if col.geometry_type == "mesh":
                     print "Mesh geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "image":
                     print "image geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "height_map":
                     print "Height map geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "plane":
                     print "Plane geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "sphere":
                     print "Sphere geometry is unsupported - SKIPPING!"
                 if col.geometry_type == "box":
                     desc = dict(
                         classname="BoxAffordanceItem",
                         Name=name,
                         uuid=newUUID(),
                         pose=p,
                         Color=[0.8, 0.8, 0.8],
                         Dimensions=map(float, col.geometry_data["size"].split(" ")),
                     )
                     self.affordanceManager.newAffordanceFromDescription(desc)
                 if col.geometry_type == "cylinder":
                     desc = dict(
                         classname="CylinderAffordanceItem",
                         Name=name,
                         uuid=newUUID(),
                         pose=p,
                         Color=[0.8, 0.8, 0.8],
                         Radius=float(col.geometry_data["radius"]),
                         Length=float(col.geometry_data["length"]),
                     )
                     self.affordanceManager.newAffordanceFromDescription(desc)
Exemple #4
0
    def addToView(self, view):

        rootT = transformUtils.getTransformFromNumpy(self.scene.rootnode.transformation)

        for node in self.scene.rootnode.children:

            t = transformUtils.getTransformFromNumpy(node.transformation)
            folder = om.addContainer(node.name)
            for i, mesh in enumerate(node.meshes):

                pd = self.getPolyDataForMesh(mesh)
                obj = vis.showPolyData(pd, "%s (mesh %d)" % (node.name, i), parent=folder, view=view)
                obj.actor.SetTexture(self.getTextureForMaterial(mesh.material))
                obj.actor.SetUserTransform(t)
Exemple #5
0
    def addToView(self, view):

        rootT = transformUtils.getTransformFromNumpy(self.scene.rootnode.transformation)

        for node in self.scene.rootnode.children:

            t = transformUtils.getTransformFromNumpy(node.transformation)
            folder = om.addContainer(node.name)
            for i, mesh in enumerate(node.meshes):

                pd = self.getPolyDataForMesh(mesh)
                obj = vis.showPolyData(pd, '%s (mesh %d)' % (node.name, i), parent=folder, view=view)
                obj.actor.SetTexture(self.getTextureForMaterial(mesh.material))
                obj.actor.SetUserTransform(t)
Exemple #6
0
    def shadowOn(self):

        if self.shadowActor:
            return

        mat =  [[1, 0, -1,  0],
                [0, 1, -1,  0],
                [0, 0,  0,  0],
                [0, 0,  0,  1]]

        shadowT = transformUtils.getTransformFromNumpy(mat)

        baseTransform = self.actor.GetUserTransform()
        if baseTransform:
            shadowT.PreMultiply()
            shadowT.Concatenate(baseTransform)

        self.shadowActor = vtk.vtkActor()
        self.shadowActor.SetMapper(self.mapper)
        self.shadowActor.SetUserTransform(shadowT)
        self.shadowActor.GetProperty().LightingOff()
        self.shadowActor.GetProperty().SetColor(0, 0, 0)

        for view in self.views:
            view.renderer().AddActor(self.shadowActor)
Exemple #7
0
    def shadowOn(self):

        if self.shadowActor:
            return

        mat =  [[1, 0, -1,  0],
                [0, 1, -1,  0],
                [0, 0,  0,  0],
                [0, 0,  0,  1]]

        shadowT = transformUtils.getTransformFromNumpy(mat)

        baseTransform = self.actor.GetUserTransform()
        if baseTransform:
            shadowT.PreMultiply()
            shadowT.Concatenate(baseTransform)

        self.shadowActor = vtk.vtkActor()
        self.shadowActor.SetMapper(self.mapper)
        self.shadowActor.SetUserTransform(shadowT)
        self.shadowActor.GetProperty().LightingOff()
        self.shadowActor.GetProperty().SetColor(0, 0, 0)

        for view in self.views:
            view.renderer().AddActor(self.shadowActor)
Exemple #8
0
    def forwardKinematicsExample(self):

        q = np.zeros(self.rigidBodyTree.get_num_positions())
        v = np.zeros(self.rigidBodyTree.get_num_velocities())
        kinsol = self.rigidBodyTree.doKinematics(q,v)

        t = self.rigidBodyTree.relativeTransform(kinsol, self.bodyNameToId['world'], self.bodyNameToId['leftFoot'])
        tt = transformUtils.getTransformFromNumpy(t)
        pos = transformUtils.transformations.translation_from_matrix(t)
        quat = transformUtils.transformations.quaternion_from_matrix(t)
Exemple #9
0
    def forwardKinematicsExample(self):

        q = np.zeros(self.rigidBodyTree.get_num_positions())
        v = np.zeros(self.rigidBodyTree.get_num_velocities())
        kinsol = self.rigidBodyTree.doKinematics(q,v)

        t = self.rigidBodyTree.relativeTransform(kinsol, self.bodyNameToId['world'], self.bodyNameToId['leftFoot'])
        tt = transformUtils.getTransformFromNumpy(t)
        pos = transformUtils.transformations.translation_from_matrix(t)
        quat = transformUtils.transformations.quaternion_from_matrix(t)
 def loadSDF(self, filename):
     sdf = pysdf.SDF(file=filename)
     for model in sdf.world.models:
         for link in model.links:
           if hasattr(link, 'submodels'):
             if len(link.submodels)>0:
               print model.name+' - This is an articulated object - SKIPPING!'
               break
           for col in link.collisions:   
             t1=transformUtils.getTransformFromNumpy(model.pose)
             t2=transformUtils.getTransformFromNumpy(link.pose)
             t3=transformUtils.getTransformFromNumpy(col.pose)
             t=t1
             t.PreMultiply()
             t.Concatenate(t2)  
             t.PreMultiply()
             t.Concatenate(t3)
             p = transformUtils.poseFromTransform(t)
             name = model.name
             color = col.color[0:3] if col.color is not None else [0.8,0.8,0.8]
             if len(link.name)>0 and link.name != model.name:
                 name+='-'+link.name
             if len(col.name)>0 and len(link.collisions)>1:
                 name+='-'+col.name
             if col.geometry_type=='mesh':
                 desc = dict(classname='MeshAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Filename=col.geometry_data['uri'], Scale=map(float, col.geometry_data['scale'].split(' ')))
                 self.affordanceManager.newAffordanceFromDescription(desc)
             if col.geometry_type=='image':
                 print 'image geometry is unsupported - SKIPPING!'
             if col.geometry_type=='height_map':
                 print 'Height map geometry is unsupported - SKIPPING!'
             if col.geometry_type=='plane':
                 print 'Plane geometry is unsupported - SKIPPING!'
             if col.geometry_type=='sphere':
                 print 'Sphere geometry is unsupported - SKIPPING!'
             if col.geometry_type=='box':
                 desc = dict(classname='BoxAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Dimensions=map(float, col.geometry_data['size'].split(' ')))
                 self.affordanceManager.newAffordanceFromDescription(desc)
             if col.geometry_type=='cylinder':
                 desc = dict(classname='CylinderAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Radius=float(col.geometry_data['radius']), Length = float(col.geometry_data['length']))
                 self.affordanceManager.newAffordanceFromDescription(desc)
    def parse_transform(transform_matrix_list):
        """
        Returns a vtkTransform matrix from column major list of matrix coefficients
        :param transform_matrix_list:
        :type transform_matrix_list:
        :return:
        :rtype:
        """

        matrix_coeffs = np.array(transform_matrix_list)  # vector of length 16
        mat = np.reshape(matrix_coeffs, [4, 4], order='F')
        return transformUtils.getTransformFromNumpy(mat)
Exemple #12
0
    def update(self, snapshot=False):
        """
        Visualizes the pointclouds set to true. This should be called from the main thread
        :return:
        :rtype:
        """

        for topic, data in self._subscribers.iteritems():

            if not data['visualize']:
                continue

            msg = data['subscriber'].lastMsg
            if msg is None:
                continue

            # get frame
            # TransformStamped
            # this might need to be called in thread
            try:
                T_W_pointcloud_stamped = self._tf_buffer.lookup_transform(
                    self._expressed_in_frame, msg.header.frame_id,
                    msg.header.stamp)
            except:
                continue

            T_W_pointcloud = ros_numpy.numpify(
                T_W_pointcloud_stamped.transform)
            T_W_pointcloud_vtk = transformUtils.getTransformFromNumpy(
                T_W_pointcloud)
            pointcloud_numpy = DirectorROSVisualizer.numpy_from_pointcloud2_msg(
                msg)
            pointcloud_vtk = vnp.getVtkPolyDataFromNumpyPoints(
                pointcloud_numpy)
            pointcloud_vtk = filterUtils.transformPolyData(
                pointcloud_vtk, T_W_pointcloud_vtk)

            data['pointcloud'] = pointcloud_vtk
            if snapshot:
                name = data["name"] + " snapshot"
                vis.showPolyData(pointcloud_vtk,
                                 name,
                                 parent=self._vis_container)
            else:
                vis.updatePolyData(pointcloud_vtk,
                                   data['name'],
                                   parent=self._vis_container)
Exemple #13
0
def testEulerBotpy():
    '''
    Test some quaternion and euler conversions with botpy
    '''

    quat = transformations.random_quaternion()
    rpy = transformations.euler_from_quaternion(quat)

    rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
    quat2 = botpy.roll_pitch_yaw_to_quat(rpy)

    mat = transformations.quaternion_matrix(quat)
    frame = transformUtils.getTransformFromNumpy(mat)
    rpy3 = transformUtils.rollPitchYawFromTransform(frame)

    print quat, quat2
    print rpy, rpy2, rpy3

    assert isQuatEqual(quat, quat2)
    assert np.allclose(rpy, rpy2)
    assert np.allclose(rpy2, rpy3)
Exemple #14
0
def testEulerBotpy():
    '''
    Test some quaternion and euler conversions with botpy
    '''

    quat = transformations.random_quaternion()
    rpy = transformations.euler_from_quaternion(quat)

    rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
    quat2 = botpy.roll_pitch_yaw_to_quat(rpy)

    mat = transformations.quaternion_matrix(quat)
    frame = transformUtils.getTransformFromNumpy(mat)
    rpy3 = transformUtils.rollPitchYawFromTransform(frame)

    print quat, quat2
    print rpy, rpy2, rpy3

    assert isQuatEqual(quat, quat2)
    assert np.allclose(rpy, rpy2)
    assert np.allclose(rpy2, rpy3)
def testIkPlan():

    ikPlanner = robotSystem.ikPlanner

    constraints = buildConstraints()
    poses = ce.getPlanPoses(constraints, ikPlanner)

    global robot

    robot = loadRigidBodyTree(getIkUrdfFilename())


    tspan = np.array([1.0, 1.0])

    handLinkNames = [ikPlanner.getHandLink('left'), ikPlanner.getHandLink('right')]
    footLinkNames = [robotSystem.directorConfig['leftFootLink'], robotSystem.directorConfig['rightFootLink']]

    leftHandBodyId = robot.FindBody(str(handLinkNames[0])).get_body_index()
    rightHandBodyId = robot.FindBody(str(handLinkNames[1])).get_body_index()
    leftFootBodyId = robot.FindBody(str(footLinkNames[0])).get_body_index()
    rightFootBodyId = robot.FindBody(str(footLinkNames[1])).get_body_index()

    lfootContactPts, rfootContactPts = getFootContactPoints()
    #lfootContactPts, rfootContactPts = = robotSystem.footstepsDriver.getContactPts()


    for i in xrange(robot.get_num_bodies()):
        body = robot.get_body(i)
        print i, body.get_name()
        assert robot.FindBodyIndex(body.get_name()) == i

    q = np.zeros(robot.get_num_positions())
    v = np.zeros(robot.get_num_velocities())
    kinsol = robot.doKinematics(q,v)

    t = robot.relativeTransform(kinsol, robot.FindBodyIndex('world'), robot.FindBodyIndex(str(footLinkNames[0])))
    tt = transformUtils.getTransformFromNumpy(t)

    pos = transformUtils.transformations.translation_from_matrix(t)
    quat = transformUtils.transformations.quaternion_from_matrix(t)



    footConstraints = []

    for linkName in footLinkNames:
        t = robotSystem.robotStateModel.getLinkFrame(linkName)
        pos, quat = transformUtils.poseFromTransform(t)

        if False and linkName == footLinkNames[0]:
            pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos + [-10,-10,0], pos+[10,10,0], tspan)
        else:
            pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos, pos, tspan)

        qc = pydrakeik.WorldQuatConstraint(robot, robot.findLink(str(linkName)).get_body_index(), quat, 0.01, tspan)
        footConstraints.append(pc)
        footConstraints.append(qc)


    qsc = pydrakeik.QuasiStaticConstraint(robot, tspan)
    qsc.setActive(True)
    qsc.setShrinkFactor(1.0)
    qsc.addContact([leftFootBodyId], lfootContactPts.transpose())
    qsc.addContact([rightFootBodyId], rfootContactPts.transpose())


    #q_seed = robot.getZeroConfiguration()
    #q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy()

    # todo, joint costs, and other options
    global options
    options = pydrakeik.IKoptions(robot)


    jointIndexMap = np.zeros(robotSystem.robotStateJointController.numberOfJoints)
    drakePositionNames = [robot.get_position_name(i) for i in xrange(robot.get_num_positions())]
    for i, name in enumerate(robotSystem.robotStateJointController.jointNames):
        jointIndexMap[i] = drakePositionNames.index(name)

    jointIndexMap = list(jointIndexMap)
    print jointIndexMap

    q_seed = np.zeros(robot.get_num_positions())
    q_seed[jointIndexMap] = robotSystem.robotStateJointController.getPose('q_nom').copy()

    # setup costs

    costs = np.ones(robot.get_num_positions())


    groupCosts = [
      ('Left Leg', 1e3),
      ('Right Leg', 1e3),
      ('Left Arm', 1),
      ('Right Arm', 1),
      ('Back', 1e4),
      ('Neck', 1e6),
      ]

    for groupName, costValue in groupCosts:
        names = robotSystem.ikPlanner.getJointGroup(groupName)
        inds = [drakePositionNames.index(name) for name in names]
        print costValue, names
        costs[inds] = costValue

    costs[drakePositionNames.index('base_x')] = 0
    costs[drakePositionNames.index('base_y')] = 0
    costs[drakePositionNames.index('base_z')] = 0
    costs[drakePositionNames.index('base_roll')] = 1e3
    costs[drakePositionNames.index('base_pitch')] = 1e3
    costs[drakePositionNames.index('base_yaw')] = 0


    print costs
    options.setQ(np.diag(costs))


    def solveAndDraw(constraints):

        #print q_seed.shape

        global results
        global constraintList
        constraintList = constraints
        results = pydrakeik.InverseKin(robot, q_seed, q_seed, constraints, options)
        pose = results.q_sol[0]

        #print results.info[0]

        pose = pose[jointIndexMap]

        robotSystem.robotStateJointController.setPose('pose_end', pose)


    def onFrameModified(obj):

        pos, quat = transformUtils.poseFromTransform(obj.transform)

        pc = pydrakeik.WorldPositionConstraint(robot, leftHandBodyId,
                                              np.zeros((3,)),
                                              pos,
                                              pos, tspan)

        qc = pydrakeik.WorldQuatConstraint(robot, leftHandBodyId,
                          quat, 0.01, tspan)

        solveAndDraw([qsc, pc, qc] + footConstraints)

        #solveAndDraw([pc])


    frameObj = vis.updateFrame(robotSystem.robotStateModel.getLinkFrame(ikPlanner.getHandLink('left')), 'left hand frame')
    frameObj.setProperty('Edit', True)
    frameObj.setProperty('Scale', 0.2)
    frameObj.connectFrameModified(onFrameModified)
Exemple #16
0
    def run(scenePointCloudOriginal,
            modelPointCloudOriginal,
            numDownsampledPoints=1000):

        # make deep copies of pointclouds so we don't touch original data
        scenePointCloud = vtk.vtkPolyData()
        scenePointCloud.DeepCopy(scenePointCloudOriginal)
        modelPointCloud = vtk.vtkPolyData()
        modelPointCloud.DeepCopy(modelPointCloudOriginal)

        # transform the data a bit
        registration = GlobalRegistrationUtils
        # use this if your point cloud contains invalid points with 0.0 range
        scenePointCloud = registration.removeOriginPoints(scenePointCloud)
        scenePointCloud = registration.shiftPointsToOrigin(scenePointCloud)
        modelPointCloud = registration.shiftPointsToOrigin(modelPointCloud)
        scaleFactor = registration.rescalePolyData(
            [scenePointCloud, modelPointCloud])

        numDownsampledPoints = min(numDownsampledPoints,
                                   scenePointCloud.GetNumberOfPoints)

        print "scaleFactor = ", scaleFactor
        print "number of scene points = ", scenePointCloud.GetNumberOfPoints()
        print "downsampled number of scene points = ", numDownsampledPoints
        print "number of model points = ", modelPointCloud.GetNumberOfPoints()

        # files where temporary output will be stored
        baseName = registration.getSandboxDir()
        modelPointCloudFile = os.path.join(baseName, 'model_data.txt')
        scenePointCloudFile = os.path.join(baseName, 'scene_data.txt')
        outputFile = os.path.join(baseName, 'goicp_output.txt')

        # write the polyData to a file so that
        registration.writePointsFile(modelPointCloud, modelPointCloudFile)
        registration.writePointsFile(scenePointCloud, scenePointCloudFile)

        goICPBaseDir = utils.getGoICPBaseDir()
        # goICPConfigFile = os.path.join(goICPBaseDir, 'demo/config.txt')
        goICPConfigFile = os.path.join(utils.getLabelFusionBaseDir(),
                                       'config/go_icp_config.txt')
        goICPBin = os.path.join(goICPBaseDir, 'build/GoICP')

        goICPArgs = [
            goICPBin, modelPointCloudFile, scenePointCloudFile,
            str(numDownsampledPoints), goICPConfigFile, outputFile
        ]

        goICPArgs = ' '.join(goICPArgs).split()

        if os.path.isfile(outputFile):
            print 'removing', outputFile
            os.remove(outputFile)

        print 'calling goicp...'
        print
        print ' '.join(goICPArgs)

        startTime = time.time()
        subprocess.check_call(goICPArgs)
        elapsedTime = time.time() - startTime
        print "GoICP took " + str(elapsedTime) + " seconds"
        data = open(outputFile).readlines()
        print data

        T = np.eye(4)
        T[:3, :3] = np.array(
            [np.fromstring(l, sep=' ') for l in [data[1], data[2], data[3]]])
        T[:3, 3] = [float(x) for x in [data[4], data[5], data[6]]]

        print T

        sceneToModelTransform = transformUtils.getTransformFromNumpy(T)

        if True:
            parent = om.getOrCreateContainer('Go-ICP')
            transformedPointCloud = filterUtils.transformPolyData(
                scenePointCloud, transformUtils.getTransformFromNumpy(T))

            vis.showPolyData(modelPointCloud,
                             'model pointcloud ',
                             color=[0, 1, 0],
                             parent=parent)
            vis.showPolyData(transformedPointCloud,
                             'aligned model pointcloud',
                             color=[1, 0, 0],
                             parent=parent)
            vis.showPolyData(scenePointCloud,
                             'scene pointcloud goicp',
                             color=[0, 1, 1],
                             parent=parent)

        return sceneToModelTransform
Exemple #17
0
 def getTransformFromFile(outputFile):
     assert os.path.isfile(outputFile)
     data = open(outputFile).readlines()
     T = np.array([np.fromstring(l, sep=' ') for l in data[2:6]])
     transform = transformUtils.getTransformFromNumpy(T)
     return transform
def loadTransformFromFile(filename):
    lines = open(filename).readlines()
    data = np.array([[float(x) for x in line.split()] for line in lines[1:]])
    return transformUtils.getTransformFromNumpy(data)