calibrate mode for pose estimation
This commit is contained in:
parent
4decc40c88
commit
cb3082ca44
8 changed files with 94008 additions and 220628 deletions
|
|
@ -4,6 +4,8 @@
|
|||
ROS 2 program for 6D Pose Estimation
|
||||
|
||||
@shalenikol release 0.3
|
||||
|
||||
!!! version for testing 17.11.2023
|
||||
"""
|
||||
|
||||
from typing import Optional
|
||||
|
|
@ -12,6 +14,9 @@ import shutil
|
|||
import json
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
#import pinocchio as pin
|
||||
#from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate
|
||||
import transforms3d as t3d
|
||||
|
||||
import rclpy
|
||||
from rclpy.lifecycle import Node
|
||||
|
|
@ -20,6 +25,7 @@ from rclpy.lifecycle import State
|
|||
from rclpy.lifecycle import TransitionCallbackReturn
|
||||
from rclpy.timer import Timer
|
||||
|
||||
#from tf.transformations import quaternion_from_euler
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from geometry_msgs.msg import Pose, TransformStamped
|
||||
|
|
@ -53,6 +59,8 @@ class PoseEstimator(Node):
|
|||
self.topicSrv = val
|
||||
elif name == "tf2_send_pose":
|
||||
self.tf2_send_pose = val
|
||||
elif name == "mesh_scale":
|
||||
self.mesh_scale = val
|
||||
|
||||
def __init__(self, node_name, **kwargs):
|
||||
"""Construct the node."""
|
||||
|
|
@ -66,8 +74,9 @@ class PoseEstimator(Node):
|
|||
self._is_camerainfo = False
|
||||
self._K = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]
|
||||
self._res = [0, 0]
|
||||
self._pose = [[1., 0., 0., 0.], [0., 0., 0.]]
|
||||
self._pose = [[1., 0., 0., 0.], [0., 0., 0.]] # pose in format "TWO" (megapose)
|
||||
self.tf2_send_pose = 0
|
||||
self.mesh_scale = 1.0
|
||||
self.megapose_model = None
|
||||
|
||||
self.nodeName = node_name
|
||||
|
|
@ -77,6 +86,7 @@ class PoseEstimator(Node):
|
|||
self.publishDelay = 2.0
|
||||
self.topicSrv = self.nodeName + "/detect6Dpose"
|
||||
self._InitService()
|
||||
self.camera_pose = self.get_camera_pose()
|
||||
|
||||
self.tmpdir = tempfile.gettempdir()
|
||||
self.mytemppath = Path(self.tmpdir) / "rbs_per"
|
||||
|
|
@ -97,28 +107,56 @@ class PoseEstimator(Node):
|
|||
self.objMeshFile = ""
|
||||
self.objPath = ""
|
||||
|
||||
def get_camera_pose(self) -> Pose:
|
||||
# <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
|
||||
p = Pose()
|
||||
p.position.x = -2.
|
||||
p.position.y = -0.55
|
||||
p.position.z = 1.44
|
||||
#R = rpyToMatrix([0, 0.5, -2.4])
|
||||
#q = pin.Quaternion()
|
||||
#q = t3d.euler.euler2quat(0., 0.5, -2.4)
|
||||
p.orientation.w = 0.9524 #q[0]
|
||||
p.orientation.x = -0.0476 #q[1]
|
||||
p.orientation.y = 0.213 #q[2]
|
||||
p.orientation.z = 0.213 #q[3]
|
||||
return p
|
||||
|
||||
def publish(self):
|
||||
"""Publish a new message when enabled."""
|
||||
self._count += 1
|
||||
|
||||
if self._pub is not None and self._pub.is_activated:
|
||||
# опубликуем результат оценки позы
|
||||
# опубликуем результат оценки позы # publish pose estimation result
|
||||
q = self._pose[0]
|
||||
t = self._pose[1]
|
||||
"""
|
||||
msgQ = self.camera_pose.orientation
|
||||
q_rot = [msgQ.w, msgQ.x, msgQ.y, msgQ.z]
|
||||
q = t3d.quaternions.qinverse(q) #q[0] = -q[0]
|
||||
q_mir = t3d.quaternions.qmult(q_rot,q)
|
||||
"""
|
||||
q_mir = q
|
||||
|
||||
p = Pose()
|
||||
"""
|
||||
p.position.x = t[0] * self.mesh_scale
|
||||
p.position.y = t[1] * self.mesh_scale
|
||||
p.position.z = t[2] * self.mesh_scale
|
||||
"""
|
||||
p.position.x = t[0]
|
||||
p.position.y = t[1]
|
||||
p.position.z = t[2]
|
||||
p.orientation.w = q[0]
|
||||
p.orientation.x = q[1]
|
||||
p.orientation.y = q[2]
|
||||
p.orientation.z = q[3]
|
||||
p.orientation.w = q_mir[0]
|
||||
p.orientation.x = q_mir[1]
|
||||
p.orientation.y = q_mir[2]
|
||||
p.orientation.z = q_mir[3]
|
||||
self._pub.publish(p)
|
||||
|
||||
if self.tf2_send_pose:
|
||||
self.tf_obj_pose(self._pose)
|
||||
self.tf_obj_pose(p.position,q_mir) #(self._pose)
|
||||
|
||||
def tf_obj_pose(self,pose):
|
||||
def tf_obj_pose(self, tr, q):
|
||||
"""
|
||||
Передача позиции объекта в tf2
|
||||
"""
|
||||
|
|
@ -128,16 +166,16 @@ class PoseEstimator(Node):
|
|||
t.header.frame_id = 'world'
|
||||
t.child_frame_id = self.objName
|
||||
# coordinates
|
||||
tr = pose[1]
|
||||
t.transform.translation.x = tr[0]
|
||||
t.transform.translation.y = tr[1]
|
||||
t.transform.translation.z = tr[2]
|
||||
#tr = pose[1]
|
||||
t.transform.translation.x = tr.x #[0]
|
||||
t.transform.translation.y = tr.y #[1]
|
||||
t.transform.translation.z = tr.z #[2]
|
||||
# rotation
|
||||
q = pose[0]
|
||||
t.transform.rotation.x = q[1] # 0
|
||||
t.transform.rotation.y = q[2] # 1
|
||||
t.transform.rotation.z = q[3] # 2
|
||||
t.transform.rotation.w = q[0] # 3
|
||||
#q = pose[0]
|
||||
t.transform.rotation.w = q[0]
|
||||
t.transform.rotation.x = q[1]
|
||||
t.transform.rotation.y = q[2]
|
||||
t.transform.rotation.z = q[3]
|
||||
# Send the transformation
|
||||
self.tf_broadcaster.sendTransform(t)
|
||||
|
||||
|
|
@ -151,7 +189,19 @@ class PoseEstimator(Node):
|
|||
TransitionCallbackReturn.FAILURE transitions to "unconfigured".
|
||||
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
|
||||
"""
|
||||
mesh_path = self.get_parameter("mesh_path").get_parameter_value().string_value
|
||||
gtpose = None
|
||||
str_param = self.get_parameter("mesh_path").get_parameter_value().string_value
|
||||
if str_param[0] == "{": # json string
|
||||
y = json.loads(str_param)
|
||||
if "mesh_path" not in y:
|
||||
self.get_logger().info("JSon 'mesh_path' not set")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
mesh_path = y["mesh_path"]
|
||||
if "gtpose" in y:
|
||||
gtpose = y["gtpose"]
|
||||
else:
|
||||
mesh_path = str_param
|
||||
|
||||
if not os.path.isfile(mesh_path):
|
||||
self.get_logger().info("Parameter 'mesh_path' not set")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
|
|
@ -179,8 +229,12 @@ class PoseEstimator(Node):
|
|||
self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 10)
|
||||
self._timer = self.create_timer(self.publishDelay, self.publish)
|
||||
|
||||
# Preload Megapose model
|
||||
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
if gtpose == None:
|
||||
# Preload Megapose model
|
||||
from megapose.scripts.run_inference_on_example import ModelPreload
|
||||
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
else:
|
||||
self._pose = [t3d.euler.euler2quat(gtpose[3], gtpose[4], gtpose[5]), gtpose[:3]]
|
||||
|
||||
self.get_logger().info('on_configure() is called.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
|
@ -254,9 +308,9 @@ class PoseEstimator(Node):
|
|||
self._res = [data.height, data.width]
|
||||
k_ = data.k
|
||||
self._K = [
|
||||
[k_[0], k_[1], k_[2]],
|
||||
[k_[3], k_[4], k_[5]],
|
||||
[k_[6], k_[7], k_[8]]
|
||||
[k_[0]*2.0, k_[1], data.width / 2.0], # k_[2]], #
|
||||
[k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
|
||||
[k_[6], k_[7], k_[8]] #self.mesh_scale]
|
||||
]
|
||||
|
||||
tPath = self.objPath / "inputs"
|
||||
|
|
@ -264,7 +318,7 @@ class PoseEstimator(Node):
|
|||
output_fn = tPath / "object_data.json"
|
||||
output_json_dict = {
|
||||
"label": self.objName,
|
||||
"bbox_modal": [2,2,self._res[1]-4,self._res[0]-4]
|
||||
"bbox_modal": [288,170,392,253] #[2,2,self._res[1]-4,self._res[0]-4]
|
||||
}
|
||||
data = []
|
||||
data.append(output_json_dict)
|
||||
|
|
@ -318,17 +372,18 @@ class PoseEstimator(Node):
|
|||
cv2.imwrite(str(self.objPath / "image_rgb.png"), current_frame)
|
||||
self._image_cnt += 1
|
||||
|
||||
# 6D pose estimation
|
||||
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
|
||||
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
run_inference_rbs(self.megapose_model)
|
||||
if self.megapose_model:
|
||||
# 6D pose estimation
|
||||
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
|
||||
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
from megapose.scripts.run_inference_on_example import run_inference_rbs
|
||||
run_inference_rbs(self.megapose_model)
|
||||
|
||||
data = self.load_result(self.objPath)
|
||||
if data[0] == "[":
|
||||
y = json.loads(data)[0]
|
||||
self._pose = y["TWO"]
|
||||
|
||||
self.get_logger().info(f"megapose: end {self._image_cnt}")
|
||||
data = self.load_result(self.objPath)
|
||||
if data[0] == "[":
|
||||
y = json.loads(data)[0]
|
||||
self._pose = y["TWO"]
|
||||
self.get_logger().info(f"megapose: end {self._image_cnt}")
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue