camera_view.py
Go to the documentation of this file.
1 # camera_view.py
2 import math
3 import numpy
4 import tf
5 import rospy
6 import imp
7 try:
8  imp.find_module("view_controller_msgs")
9 except:
10  import roslib; roslib.load_manifest('jsk_teleop_joy')
11 
12 from view_controller_msgs.msg import CameraPlacement
13 
14 class CameraView():
15  def __init__(self):
16  self.yaw = 0.0
17  self.pitch = 0.0
18  #self.roll = 0.0
19  self.distance = 2.0
20  self.focus = numpy.array((0, 0, 0))
21  self.z_up = numpy.array((0, 0, 1))
22 
23  @staticmethod
24  def createFromCameraPlacement(camera_placement):
25  instance = CameraView()
26  ceye = numpy.array((camera_placement.eye.point.x,
27  camera_placement.eye.point.y,
28  camera_placement.eye.point.z))
29  cfocus = numpy.array((camera_placement.focus.point.x,
30  camera_placement.focus.point.y,
31  camera_placement.focus.point.z))
32  instance.focus = cfocus
33  viewdir = ceye - cfocus
34  instance.distance = numpy.linalg.norm(viewdir)
35 
36  instance.z_up[0] = camera_placement.up.vector.x
37  instance.z_up[1] = camera_placement.up.vector.y
38  instance.z_up[2] = camera_placement.up.vector.z
39 
40  instance.pitch = math.asin(viewdir[2]/instance.distance)
41  instance.yaw = math.atan2(viewdir[1], viewdir[0])
42 
43  return instance
44 
45  def viewPoint(self):
46  p = numpy.array((self.distance * math.cos(self.yaw) * math.cos(self.pitch) + self.focus[0],
47  self.distance * math.sin(self.yaw) * math.cos(self.pitch) + self.focus[1],
48  self.distance * math.sin(self.pitch) + self.focus[2]))
49  return p
50  def cameraOrientation(self):
51  OE = self.viewPoint()
52  f = self.focus - OE # z
53  f = f / tf.transformations.vector_norm(f)
54  u = numpy.array((0, 0, 1)) #not aligned y
55  r = numpy.cross(u, f) # x
56  r = r / tf.transformations.vector_norm(r)
57  uy = numpy.cross(f, r)
58  uy = uy / tf.transformations.vector_norm(uy)
59  m = tf.transformations.identity_matrix()[:3, :3] #rotation matrix
60  m[0, 0] = r[0]
61  m[1, 0] = r[1]
62  m[2, 0] = r[2]
63  m[0, 1] = uy[0]
64  m[1, 1] = uy[1]
65  m[2, 1] = uy[2]
66  m[0, 2] = f[0]
67  m[1, 2] = f[1]
68  m[2, 2] = f[2]
69  return m
70  def cameraPlacement(self):
71  #TIME = 0.05
72  #TIME = 1.0 / 40 * 2.0
73  TIME = 1.0 / 30
74  view_point = self.viewPoint()
75  placement = CameraPlacement()
76  placement.interpolation_mode = CameraPlacement.LINEAR
77  #placement.interpolation_mode = CameraPlacement.SPHERICAL
78  placement.time_from_start = rospy.Duration(TIME)
79  # placement.eye.header.stamp = rospy.Time(0.0)
80  placement.eye.header.stamp = rospy.Time.now()
81  placement.eye.point.x = view_point[0]
82  placement.eye.point.y = view_point[1]
83  placement.eye.point.z = view_point[2]
84  placement.focus.header.stamp = rospy.Time.now()
85  placement.focus.point.x = self.focus[0]
86  placement.focus.point.y = self.focus[1]
87  placement.focus.point.z = self.focus[2]
88  placement.up.header.stamp = rospy.Time.now()
89  placement.up.vector.x = self.z_up[0]
90  placement.up.vector.y = self.z_up[1]
91  placement.up.vector.z = self.z_up[2]
92  placement.mouse_interaction_mode = CameraPlacement.ORBIT
93  return placement
jsk_teleop_joy.camera_view.CameraView
Definition: camera_view.py:14
jsk_teleop_joy.camera_view.CameraView.pitch
pitch
Definition: camera_view.py:17
jsk_teleop_joy.camera_view.CameraView.cameraOrientation
def cameraOrientation(self)
Definition: camera_view.py:50
jsk_teleop_joy.camera_view.CameraView.createFromCameraPlacement
def createFromCameraPlacement(camera_placement)
Definition: camera_view.py:24
jsk_teleop_joy.camera_view.CameraView.z_up
z_up
Definition: camera_view.py:21
jsk_teleop_joy.camera_view.CameraView.distance
distance
Definition: camera_view.py:19
jsk_teleop_joy.camera_view.CameraView.__init__
def __init__(self)
Definition: camera_view.py:15
jsk_teleop_joy.camera_view.CameraView.focus
focus
Definition: camera_view.py:20
jsk_teleop_joy.camera_view.CameraView.cameraPlacement
def cameraPlacement(self)
Definition: camera_view.py:70
jsk_teleop_joy.camera_view.CameraView.viewPoint
def viewPoint(self)
Definition: camera_view.py:45
jsk_teleop_joy.camera_view.CameraView.yaw
yaw
Definition: camera_view.py:16


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:49