33 lines
1.3 KiB
Python
33 lines
1.3 KiB
Python
from utils.data_load import DataLoadUtil
|
|
from utils.control_util import ControlUtil
|
|
from utils.view_util import ViewUtil
|
|
from utils.pts_util import PtsUtil
|
|
from utils.communicate_util import CommunicateUtil
|
|
import numpy as np
|
|
import os
|
|
if __name__ == "__main__":
|
|
idx = "2"
|
|
ControlUtil.connect_robot()
|
|
ControlUtil.franka_reset()
|
|
root_path = "/home/yan20/nbv_rec/project/franka_control/temp_output/cad_model_world"
|
|
frame_path = os.path.join(root_path, idx)
|
|
cam_info = DataLoadUtil.load_cam_info(frame_path, binocular=True)
|
|
render_camL_to_world = cam_info["cam_to_world"]
|
|
render_camO_to_world = cam_info["cam_to_world_O"]
|
|
L_to_O = np.dot(np.linalg.inv(render_camO_to_world), render_camL_to_world)
|
|
|
|
ControlUtil.set_pose(render_camO_to_world)
|
|
|
|
real_camO_to_world = ControlUtil.get_pose()
|
|
real_camL_to_world = np.dot(real_camO_to_world,L_to_O)
|
|
view_data = CommunicateUtil.get_view_data(init=True)
|
|
cam_pts = ViewUtil.get_pts(view_data)
|
|
np.savetxt(f"cam_pts_{idx}.txt", cam_pts)
|
|
world_pts = PtsUtil.transform_point_cloud(cam_pts, render_camL_to_world)
|
|
print(real_camL_to_world)
|
|
np.savetxt(f"world_pts_{idx}.txt", world_pts)
|
|
# import ipdb;ipdb.set_trace()
|
|
|
|
# view_data = CommunicateUtil.get_view_data()
|
|
# cam_pts = ViewUtil.get_pts(view_data)
|
|
# np.savetxt(f"cam_pts_{idx}.txt", cam_pts) |