jsk_teleop_joy: camera_view.py Source File

Go to the documentation of this file.
00001 # camera_view.py
00002 import math
00003 import numpy
00004 import tf
00005 import rospy
00006 import imp
00007 try:
00008   imp.find_module("view_controller_msgs")
00009 except:
00010   import roslib; roslib.load_manifest('jsk_teleop_joy')
00011 
00012 from view_controller_msgs.msg import CameraPlacement
00013 
00014 class CameraView():
00015   def __init__(self):
00016     self.yaw = 0.0
00017     self.pitch = 0.0
00018     #self.roll = 0.0
00019     self.distance = 2.0
00020     self.focus = numpy.array((0, 0, 0))
00021     self.z_up = numpy.array((0, 0, 1))
00022 
00023   @staticmethod
00024   def createFromCameraPlacement(camera_placement):
00025     instance = CameraView()
00026     ceye = numpy.array((camera_placement.eye.point.x,
00027                         camera_placement.eye.point.y,
00028                         camera_placement.eye.point.z))
00029     cfocus = numpy.array((camera_placement.focus.point.x,
00030                           camera_placement.focus.point.y,
00031                           camera_placement.focus.point.z))
00032     instance.focus = cfocus
00033     viewdir = ceye - cfocus
00034     instance.distance = numpy.linalg.norm(viewdir)
00035 
00036     instance.z_up[0] = camera_placement.up.vector.x
00037     instance.z_up[1] = camera_placement.up.vector.y
00038     instance.z_up[2] = camera_placement.up.vector.z
00039 
00040     instance.pitch = math.asin(viewdir[2]/instance.distance)
00041     instance.yaw   = math.atan2(viewdir[1], viewdir[0])
00042 
00043     return instance
00044 
00045   def viewPoint(self):
00046     p = numpy.array((self.distance * math.cos(self.yaw) * math.cos(self.pitch) + self.focus[0],
00047                      self.distance * math.sin(self.yaw) * math.cos(self.pitch) + self.focus[1],
00048                      self.distance *                      math.sin(self.pitch) + self.focus[2]))
00049     return p
00050   def cameraOrientation(self):
00051     OE = self.viewPoint()
00052     f = self.focus - OE # z
00053     f = f / tf.transformations.vector_norm(f)
00054     u = numpy.array((0, 0, 1))            #not aligned y
00055     r = numpy.cross(u, f) # x
00056     r = r / tf.transformations.vector_norm(r)
00057     uy = numpy.cross(f, r)
00058     uy = uy / tf.transformations.vector_norm(uy)
00059     m = tf.transformations.identity_matrix()[:3, :3]   #rotation matrix
00060     m[0, 0] = r[0]
00061     m[1, 0] = r[1]
00062     m[2, 0] = r[2]
00063     m[0, 1] = uy[0]
00064     m[1, 1] = uy[1]
00065     m[2, 1] = uy[2]
00066     m[0, 2] = f[0]
00067     m[1, 2] = f[1]
00068     m[2, 2] = f[2]
00069     return m
00070   def cameraPlacement(self):
00071     #TIME = 0.05
00072     #TIME = 1.0 / 40 * 2.0
00073     TIME = 1.0 / 30
00074     view_point = self.viewPoint()
00075     placement = CameraPlacement()
00076     placement.interpolation_mode = CameraPlacement.LINEAR
00077     #placement.interpolation_mode = CameraPlacement.SPHERICAL
00078     placement.time_from_start = rospy.Duration(TIME)
00079     # placement.eye.header.stamp = rospy.Time(0.0)
00080     placement.eye.header.stamp = rospy.Time.now()
00081     placement.eye.point.x = view_point[0]
00082     placement.eye.point.y = view_point[1]
00083     placement.eye.point.z = view_point[2]
00084     placement.focus.header.stamp = rospy.Time.now()
00085     placement.focus.point.x = self.focus[0]
00086     placement.focus.point.y = self.focus[1]
00087     placement.focus.point.z = self.focus[2]
00088     placement.up.header.stamp = rospy.Time.now()
00089     placement.up.vector.x = self.z_up[0]
00090     placement.up.vector.y = self.z_up[1]
00091     placement.up.vector.z = self.z_up[2]
00092     placement.mouse_interaction_mode = CameraPlacement.ORBIT
00093     return placement