def test():
from Camera.UnrealCVCamera import UnrealCVCamera
from MotionPlatform.PlatformUnrealCV import PlatformUnrealCV
from UnrealCVBase.UnrealCVEnv import UnrealCVEnv
initPose = Pose3.from6D(np.array([-500, 500, -1000, 0, 0, 0])) # sofa
#X = Pose3.fromCenter6D([0, 0, 0, 1, 2, 3])
X = Pose3.fromCenter6D([0, 0, 0, 0, 0, 0])
unrealbase = UnrealCVEnv(init_pose=initPose)
camera = UnrealCVCamera(
unreal_env=unrealbase,
cameraCalib=CameraCalibration()) # type: CameraBase
platform = PlatformUnrealCV(unreal_env=unrealbase, X=X)
myACR = ACRBisection(camera=camera, platform=platform)
myACR.openAll()
ref_image, ref_image_depth = myACR.camera.getImage()
pose = Pose3.fromCenter6D([10, 0, 8, 1.2, 0, -1.3])
platform.movePose(movingPose=pose)
directory = "D:/temp/acr"
if not os.path.exists(directory):
os.mkdir(directory)
img_path = os.path.join(directory, "rgb_ref.png")
img_depth_path = os.path.join(directory, "depth_ref.png")
cv2.imwrite(img_path, ref_image)
cv2.imwrite(img_depth_path, ref_image_depth)
myACR.initSettings(data_dir=directory, refImage=ref_image)
# input('Press to continue...')
myACR.relocation()
def initSettings(self, data_dir, refImage: np.ndarray,
refImage_depth: np.ndarray):
self.step = 0
self.curPose = Pose3.from6D([100, 100, 100, 100, 100, 100
]) # set initial pose to be a large value
self.curAFD = 100
self.data_dir = data_dir
self.refImage = refImage
self.refImage_depth = refImage_depth
def initSettings(self, data_dir, refImage: np.ndarray):
self.step = 0
self.curPose = Pose3.from6D([100, 100, 100, 100, 100, 100
]) # set initial pose to be a large value
self.curAFD = 100
self.cur_S = self.init_S
self.cur_angle_bound = self.init_angle_bound
self.data_dir = data_dir
self.refImage = refImage
def computeFnorm(poseList):
num = len(poseList)
fnorm_list = []
for i in range(0, num):
pose = poseList[i] # type:Pose3
se3_error = pose.toSE3() - Pose3.from6D([0, 0, 0, 0, 0, 0]).toSE3()
fnorm = np.linalg.norm(se3_error)
fnorm_list.append(fnorm)
return fnorm_list
def move_relative_pose_hand(self, RP):
print("MOVE relative pose with hand in ACRBaseUnreal")
X_6D = [10, 10, 10, 8.0416, 9.2526, 8.0416]
XH2E = Pose3.from6D(X_6D)
Eye = self.get_pose()
Hand = XH2E.inverse().compose(Eye)
Hand2 = RP.compose(Hand)
Eye2 = XH2E.compose(Hand2)
relative_pose3_for_eye = XH2E.compose(RP).compose(XH2E.inverse())
Eye2_ = relative_pose3_for_eye.compose(Eye)
assert np.allclose(Eye2.rotation(), Eye2_.rotation())
assert np.allclose(Eye2.center(), Eye2_.center())
tar_pose_move_hand = Eye2.toCenter6D()
self.set_pose_center6D(*tar_pose_move_hand)
return True