snsd0805/PokeFusion
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: rotate pokemon

Browse Source
This commit is contained in:
Ting-Jun Wang 2023-12-18 19:29:31 +08:00
parent 7530b0194e
commit a35e2e0a20
Signed by: snsd0805
GPG Key ID: 48D331A3D6160354
2 changed files with 34 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
main.py
View File

@ -277,8 +277,8 @@ if __name__ == '__main__':
paper_ccs_point = np.concatenate(new_approx, axis=0, dtype=np.float32)
# 畫邊緣 & 四點
# plot_corner_points(frame, new_approx)
# cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2) # 繪製輪廓
plot_corner_points(frame, new_approx)
cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2) # 繪製輪廓
# 算 rotaion & translation
success, rotation_vector, translation_vector = cv2.solvePnP(PAPER_WCS_POINT, paper_ccs_point, \
@ -319,21 +319,20 @@ if __name__ == '__main__':
if key == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀)
break
if key == ord(' '):
print("Clear previous approxes")
pokemon = new_pokemon()
previous_approx = []
if key == ord('w'):
print("walk forward")
pokemon.walk_forward()
if key == ord('s'):
print("walk back")
pokemon.walk_backward()
if key == ord('s'):
pokemon.walk_forward()
if key == ord('a'):
print("walk left")
pokemon.walk_left()
if key == ord('d'):
print("walk right")
pokemon.walk_right()
if key == ord('o'):
pokemon.rotate_right()
if key == ord('p'):
pokemon.rotate_left()
cap.release()
@ -341,7 +340,6 @@ if __name__ == '__main__':
# 畫 camera pose
'''
M_inv = []
for index in range(len(rotation_vectors)):
R = rotation_vectors[index]
@ -352,7 +350,7 @@ if __name__ == '__main__':
# load Pokemon points
visualization(M_inv, pokemon)
'''

32
pokemon.py
View File

@ -1,6 +1,6 @@
import open3d as o3d
import numpy as np
from math import pi
class Pokemon():
def __init__(self, pokemon_type, animal):
self.type = pokemon_type
@ -10,7 +10,8 @@ class Pokemon():
self.x = 7
self.y = 10
self.z = 5
self.theta = 2*pi
self.accum_rotate = np.array([[1, 0, 0],[0, 1, 0],[0,0,1]])
self.cloudpoints = {
'points': None,
'colors': None,
@ -56,15 +57,30 @@ class Pokemon():
self.y += 2
self.cal_position()
def rotate_left(self):
self.theta += pi/4
self.cal_position()
def rotate_right(self):
self.theta -= pi/4
self.cal_position()
def cal_position(self):
transform_matrix = np.array([
[self.scalar, 0, 0, 0],
[0, 0, self.scalar, 0],
[0, self.scalar, 0, 0],
[self.x, self.y, self.z, 1]]) # shift (前後, 左右, 高度)
[np.cos(self.theta)*self.scalar, -np.sin(self.theta)*self.scalar,0, 0],
[0,0 , self.scalar, 0],
[np.sin(self.theta)*self.scalar, np.cos(self.theta)*self.scalar, 0, 0],
[self.x, self.y, self.z, 1]]) # rotate (x, y, z, theta)
pokemon_points = self.cloudpoints['points']
'''
transform_matrix[3, :3] = transform_matrix[3, :3] @ self.accum_rotate
tmp = transform_matrix[[0,2, 1], :3]
self.accum_rotate = self.accum_rotate @ tmp
'''
ones = np.ones((pokemon_points.shape[0], 1))
pokemon_points = np.concatenate([pokemon_points, ones], axis=1)
pokemon_points = pokemon_points @ transform_matrix
@ -72,7 +88,9 @@ class Pokemon():
self.wcs_point = pokemon_points
def get_position(self):
# return self.cloudpoints['points']
return self.wcs_point
def get_colors(self):
return self.cloudpoints['colors']