$search

generate_views_vfh.py

Go to the documentation of this file.

#!/usr/bin/env python

#
# Software License Agreement (BSD License)
#
#  Copyright (c) 2010, Willow Garage, Inc.
#  All rights reserved.
#
#  Redistribution and use in source and binary forms, with or without
#  modification, are permitted provided that the following conditions
#  are met:
#
#   * Redistributions of source code must retain the above copyright
#     notice, this list of conditions and the following disclaimer.
#   * Redistributions in binary form must reproduce the above
#     copyright notice, this list of conditions and the following
#     disclaimer in the documentation and/or other materials provided
#     with the distribution.
#   * Neither the name of Willow Garage, Inc. nor the names of its
#     contributors may be used to endorse or promote products derived
#     from this software without specific prior written permission.
#
#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#  POSSIBILITY OF SUCH DAMAGE.
#
# \author Aitor Aldoma
#
# Receives two parameters:
#               * ply_file path
#               * model_id
# and computes stable planes, create views and the associated pose for each view
# of the model and starts the normal computation + vfh descriptor for each one
# of the views.

import roslib
roslib.load_manifest('vfh_recognizer_db')
import rospy
import sys
import vtk
import os
import tf.transformations as tfTransformations
import numpy
from geometry_msgs.msg import Pose
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header, Int32 as stdMsgInt32
from vfh_recognizer_db.srv import *

import roslib.packages
sys.path.append(roslib.packages.get_pkg_dir('vfh_recognition') + "/nodes")
from utils.tesselatedSphere import TesselatedSphere
import views_generator.views_generator as views_generator
from utils.visualize import Visualizer

#this will be later in a separate file
import ctypes
import math
import struct

#sys.path.append("..")
#from utils.tesselatedSphere import TesselatedSphere 
from utils.visualize import Visualizer
        
_DATATYPES = {}
_DATATYPES[PointField.INT8]    = ('b', 1)
_DATATYPES[PointField.UINT8]   = ('B', 1)
_DATATYPES[PointField.INT16]   = ('h', 2)
_DATATYPES[PointField.UINT16]  = ('H', 2)
_DATATYPES[PointField.INT32]   = ('i', 4)
_DATATYPES[PointField.UINT32]  = ('I', 4)
_DATATYPES[PointField.FLOAT32] = ('f', 4)
_DATATYPES[PointField.FLOAT64] = ('d', 8)

def create_cloud_xyz32(header,points):
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
                        PointField('y', 4, PointField.FLOAT32, 1),
                        PointField('z', 8, PointField.FLOAT32, 1)]
        return create_cloud(header, fields, points)
        
def create_cloud(header, fields, points):
        cloud = PointCloud2()
        cloud.header       = header
        cloud.height       = 1
        cloud.width        = len(points)
        cloud.is_dense     = False
        cloud.is_bigendian = False
        cloud.fields       = fields
        fmt                = _get_struct_fmt(cloud)
        cloud_struct       = struct.Struct(fmt)
        cloud.point_step   = cloud_struct.size
        cloud.row_step     = cloud_struct.size * cloud.width

        buffer = ctypes.create_string_buffer(cloud_struct.size * cloud.width)

        point_step, pack_into = cloud.point_step, cloud_struct.pack_into
        offset = 0
        for p in points:
                pack_into(buffer, offset, *p)
                offset += point_step

        cloud.data = buffer.raw

        return cloud
        
def _get_struct_fmt(cloud, field_names=None):
        fmt = '>' if cloud.is_bigendian else '<'

        offset = 0
        for field in (f for f in sorted(cloud.fields, key=lambda f: f.offset) if field_names is None or f.name in field_names):
                if offset < field.offset:
                        fmt += 'x' * (field.offset - offset)
                        offset = field.offset
                if field.datatype not in _DATATYPES:
                        print >> sys.stderr, 'Skipping unknown PointField datatype [%d]' % field.datatype
                else:
                        datatype_fmt, datatype_length = _DATATYPES[field.datatype]
                        fmt    += field.count * datatype_fmt
                        offset += field.count * datatype_length

        return fmt

#check that the transformations are done correctly...
def vtkToNumpy(vtk_matrix):
        np_matrix = numpy.zeros((4,4))
        for i in xrange(4):
                for j in xrange(4):
                        np_matrix[i,j] = float(vtk_matrix.GetElement(i,j))
        
        return np_matrix

def numPyToVTK(numpy_mat):
        vtk_matrix = vtk.vtkMatrix4x4()
        for i in xrange(4):
                for j in xrange(4):
                         vtk_matrix.SetElement(i,j,numpy_mat[i,j])
        
        return vtk_matrix

#Transform a vtkMatrix4x4 to geometry_msgs/Pose
#Quaternions ix+jy+kz+w are represented as [x, y, z, w].

def vtkTransformToPose(transform):
        #print transform
        R = vtkToNumpy(transform)
        #print R #numpy matrix from vtkMatrix
        #print "R print"
        q = tfTransformations.quaternion_from_matrix(R)  #get quaternion from rotation matrix
        #print q
        #rotFromQuaternion = tfTransformations.quaternion_matrix(q) #get rotation matrix from quaternion
        #print rotFromQuaternion
        #print "RotationMatrix from quatenion print"
        pose = Pose()
        pose.orientation.x = q[0]
        pose.orientation.y = q[1]
        pose.orientation.z = q[2]
        pose.orientation.w = q[3]
        pose.position.x = R[0,3]
        pose.position.y = R[1,3]
        pose.position.z = R[2,3]
        #print pose
        return pose

#Transform a pose to a vtkTransforom

def poseToVTKTransform(pose):
        quaternion = numpy.array((pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w))
        R = tfTransformations.quaternion_matrix(quaternion)
        R[0,3] = pose.position.x
        R[1,3] = pose.position.y        
        R[2,3] = pose.position.z
        matrixTrans = numPyToVTK(R)
        trans = vtk.vtkTransform()
        trans.SetMatrix(matrixTrans)
        return trans

def scaleModel(filename, factor):
        reader1 = vtk.vtkPLYReader()
        reader1.SetFileName(filename)

        trans = vtk.vtkTransform()
        trans.Scale(factor,factor,factor)
        transFilter1 = vtk.vtkTransformFilter()
        transFilter1.SetTransform(trans)
        transFilter1.SetInputConnection(reader1.GetOutputPort())

        mapper1 = vtk.vtkPolyDataMapper()
        mapper1.SetInputConnection(transFilter1.GetOutputPort())
        mapper1.Update()

        (before, sep, after) = filename.rpartition("/")
        output_filename = '/tmp/' + after
        print "Writting scaled model in:", output_filename, " scale_factor:", scale_factor

        writer = vtk.vtkPLYWriter()
        writer.SetFileName(output_filename)
        writer.SetInput(mapper1.GetInput())
        writer.Write()

        return output_filename

#sys.argv[1] filename of the original model (not scaled)
#sys.argv[2] scaled_model_id
#sys.argv[3] scale_factor

if __name__ == '__main__':
        if len(sys.argv) < 2:
                print "ERROR: ply file is missing..."
        else:
                filename = sys.argv[1]

        if len(sys.argv) < 3:
                print "ERROR: scale_model_id is missing..."
        else:
                scale_model_id = int(sys.argv[2])

        if len(sys.argv) < 4:
                print "ERROR: scale_factor is missing..., using 1.0"
                scale_factor = 1.0
        else:
                scale_factor = float(sys.argv[3])

        iteration = int(sys.argv[4])
        #scale the model and overwrite filename
        orig_file = filename
        output_filename = scaleModel(filename, scale_factor)
        filename = output_filename
        #quit()

        #Generation of stable planes
        #the stable planes are stored in the same directory where the ply files are found
        #under tangent_planes

        STABLE_PLANES = False
        if STABLE_PLANES:
                sphere = TesselatedSphere(1, False)
                sphere.construct()
        
                if os.path.exists(filename):
                        sphere.resetPolygons()
                        groundPlaneFunction(filename,False, sphere)
                else:
                        print "ERROR: File does not exist"

        idMessage = stdMsgInt32()
        idMessage.data = int(sys.argv[2])

        iterationMessage = stdMsgInt32()
        iterationMessage.data = int(sys.argv[4])

        #if False:      
        #Once the stable planes are generated, we can start generating the views
        #The views_generator returns a list of views (virtual camera coordinates)
        #and a transformation that transforms from model coordinates to the virtual 
        #camera coordinates. Virtual camera coordinates map the REP 103 optical 
        #frame coordinate system (see http://www.ros.org/reps/rep-0103.html)
        #[pcd_views, transforms] = views_generator.sample_views(filename)
        print "Going to generate views"
        [pcd_views, transforms] = views_generator.sample_views_tesselated_sphere(filename)

        if len(pcd_views) != len(transforms):
                print "ERROR: Number of views does not match the number of transformations"

        #Loop over views + pose
        #and call service that computes normals, VFH and store them in the database together with the pose
        for i in xrange(len(pcd_views)):
                view = pcd_views[i]
                trans = transforms[i]
                print "Number of points:", len(view)
                #transform to ROS messages to be sent to the service
                # # view = sensor_msgs/PointCloud2
                # # trans = geometry_msgs/Pose
                pc2_view = create_cloud_xyz32(Header(), view)
                pose_trans = vtkTransformToPose(trans) #transform vtk to pose message, cannot deal with scale...
                trans2 = poseToVTKTransform(pose_trans)

                rospy.wait_for_service('normals_vfh_db_estimator')
                try:
                                nvfhdb = rospy.ServiceProxy('normals_vfh_db_estimator', normals_vfh_db_estimator)
                                resp = nvfhdb(pc2_view, pose_trans, idMessage, iterationMessage)
                except rospy.ServiceException, e:
                                print "Service call failed: %s"%e

        #check for duplicate views
        #rospy.wait_for_service('remove_duplicate_views')
        #try:
        #               rdviews = rospy.ServiceProxy('remove_duplicate_views', remove_duplicate_views)
        #               resp = rdviews(idMessage, iterationMessage)
        #except rospy.ServiceException, e:
        #               print "remove_duplicate_views Service call failed: %s"%e

---

vfh_recognizer_db
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:26:29 2013