ZED pose and robot pose not aligned

Software ROS1
1

Hi, I am working now with a ZED2 camera that is attached to a differential drive robot that has an accurate wheel encoders to calculate its pose. The goal of my work is to get the position of the tracked person object by the camera that matches the position of the robot in the world coordinate frame.

However there is a problem. Even though at initial condition, the position of the person matches the position of the robot in the world, after moving the robot for some time, the ZED2 camera gives shifted position of the human compare to the robot position. This is because the true pose of the camera is different than that what the camera think (might be from a noisy IMU?). The output of the person position is already set to world coordinate frame fyi.

My current solution to correct the tracked position is by comparing the pose of the ZED2 camera and the robot and apply translation for each axis (since we consider the robot pose is the ground truth). However it would be nice to set the ZED2 pose the same as the robot pose because we would not now what would happen if the error between the two poses gets too large.

My question is, is there a way to somehow override the pose of the camera ? I have seen the service called /zed2/zed_node/set_pose from the zed_ros_wrapper package but I cant use that since my program access the camera and the node that publish that services also access the camera. You cant have two different process access the same camera right?

Thank you beforehand.

2

Hi @Krisna
you have three possible solutions:

  1. disable publish_map and publish_tf and provide the map → odom → base_link TF chain from other nodes.
  2. disable publish_map and fuse the odometry information provided by the camera with other odometry sources by using a Kalman Filter.
  3. trust only the odometry and pose published by the camera and disable all the others.

PS also calling the setPose service is a possible solution, I have not well understood why you can’t.

3

Hi again and thank you for your answer.
So basically i’ve created a python script which access the camera. and when I run the zed_node by using the command: roslaunch zed_wrapper zed2.launch (because this is the node that provide the setPose service), the printline said
*** Opening ZED 2 …
it continues by saying
ZED connection → CAMERA STREAM FAILED TO START

Is there any python example which can setPose the camera? so far I only see the code below to get the pose.
state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
py_translation = sl.Translation()
tx = round(zed_pose.get_translation(py_translation).get()[0], 3)
ty = round(zed_pose.get_translation(py_translation).get()[1], 3)
tz = round(zed_pose.get_translation(py_translation).get()[2], 3)

Thank you beforehand

4

Can you please copy and paste the full console log?

Is the ZED_Diagnostic command reporting errors?

5

Here is the output log when i run roslaunch zed_wrapper zed2.launch.
this happens whenever i run my python script first (given below)

SUMMARY

PARAMETERS

  • /rosdistro: melodic
  • /rosversion: 1.14.13
  • /zed2/zed2_description: <?xml version="1…
  • /zed2/zed_node/auto_exposure_gain: True
  • /zed2/zed_node/auto_whitebalance: True
  • /zed2/zed_node/brightness: 4
  • /zed2/zed_node/contrast: 4
  • /zed2/zed_node/depth/depth_downsample_factor: 0.5
  • /zed2/zed_node/depth/depth_stabilization: 1
  • /zed2/zed_node/depth/max_depth: 20.0
  • /zed2/zed_node/depth/min_depth: 0.3
  • /zed2/zed_node/depth/openni_depth_mode: False
  • /zed2/zed_node/depth/quality: 1
  • /zed2/zed_node/depth/sensing_mode: 0
  • /zed2/zed_node/depth_confidence: 30
  • /zed2/zed_node/depth_texture_conf: 100
  • /zed2/zed_node/exposure: 100
  • /zed2/zed_node/gain: 100
  • /zed2/zed_node/gamma: 8
  • /zed2/zed_node/general/base_frame: base_link
  • /zed2/zed_node/general/camera_flip: False
  • /zed2/zed_node/general/camera_model: zed2
  • /zed2/zed_node/general/camera_name: zed2
  • /zed2/zed_node/general/gpu_id: -1
  • /zed2/zed_node/general/grab_frame_rate: 15
  • /zed2/zed_node/general/resolution: 2
  • /zed2/zed_node/general/self_calib: True
  • /zed2/zed_node/general/serial_number: 0
  • /zed2/zed_node/general/svo_compression: 2
  • /zed2/zed_node/general/verbose: False
  • /zed2/zed_node/general/zed_id: 0
  • /zed2/zed_node/hue: 0
  • /zed2/zed_node/mapping/clicked_point_topic: /clicked_point
  • /zed2/zed_node/mapping/fused_pointcloud_freq: 1.0
  • /zed2/zed_node/mapping/mapping_enabled: False
  • /zed2/zed_node/mapping/max_mapping_range: -1
  • /zed2/zed_node/mapping/resolution: 0.05
  • /zed2/zed_node/object_detection/body_fitting: False
  • /zed2/zed_node/object_detection/confidence_threshold: 50
  • /zed2/zed_node/object_detection/max_range: 15.0
  • /zed2/zed_node/object_detection/mc_animal: True
  • /zed2/zed_node/object_detection/mc_bag: True
  • /zed2/zed_node/object_detection/mc_electronics: True
  • /zed2/zed_node/object_detection/mc_fruit_vegetable: True
  • /zed2/zed_node/object_detection/mc_people: True
  • /zed2/zed_node/object_detection/mc_sport: True
  • /zed2/zed_node/object_detection/mc_vehicle: True
  • /zed2/zed_node/object_detection/model: 0
  • /zed2/zed_node/object_detection/object_tracking_enabled: True
  • /zed2/zed_node/object_detection/od_enabled: False
  • /zed2/zed_node/point_cloud_freq: 10.0
  • /zed2/zed_node/pos_tracking/area_memory: True
  • /zed2/zed_node/pos_tracking/area_memory_db_path: zed_area_memory.area
  • /zed2/zed_node/pos_tracking/fixed_z_value: 0.0
  • /zed2/zed_node/pos_tracking/floor_alignment: False
  • /zed2/zed_node/pos_tracking/imu_fusion: True
  • /zed2/zed_node/pos_tracking/init_odom_with_first_valid_pose: True
  • /zed2/zed_node/pos_tracking/initial_base_pose: [0.0, 0.0, 0.0, 0…
  • /zed2/zed_node/pos_tracking/map_frame: map
  • /zed2/zed_node/pos_tracking/odometry_frame: odom
  • /zed2/zed_node/pos_tracking/path_max_count: -1
  • /zed2/zed_node/pos_tracking/path_pub_rate: 2.0
  • /zed2/zed_node/pos_tracking/pos_tracking_enabled: True
  • /zed2/zed_node/pos_tracking/publish_map_tf: True
  • /zed2/zed_node/pos_tracking/publish_tf: True
  • /zed2/zed_node/pos_tracking/save_area_memory_db_on_exit: False
  • /zed2/zed_node/pos_tracking/two_d_mode: False
  • /zed2/zed_node/pub_frame_rate: 15.0
  • /zed2/zed_node/saturation: 4
  • /zed2/zed_node/sensors/max_pub_rate: 200.0
  • /zed2/zed_node/sensors/publish_imu_tf: True
  • /zed2/zed_node/sensors/sensors_timestamp_sync: False
  • /zed2/zed_node/sharpness: 4
  • /zed2/zed_node/stream:
  • /zed2/zed_node/svo_file:
  • /zed2/zed_node/video/extrinsic_in_camera_frame: True
  • /zed2/zed_node/video/img_downsample_factor: 0.5
  • /zed2/zed_node/whitebalance_temperature: 42

NODES
/zed2/
zed2_state_publisher (robot_state_publisher/robot_state_publisher)
zed_node (zed_wrapper/zed_wrapper_node)

ROS_MASTER_URI=http://locobot.local:11311

process[zed2/zed2_state_publisher-1]: started with pid [22549]
process[zed2/zed_node-2]: started with pid [22551]
[ INFO] [1668675513.732923335]: Initializing nodelet with 4 worker threads.
[ INFO] [1668675513.905998637]: ********** Starting nodelet ‘/zed2/zed_node’ **********
[ INFO] [1668675513.906423569]: SDK version : 3.7.4
[ INFO] [1668675513.906686580]: *** GENERAL PARAMETERS ***
[ INFO] [1668675513.910268853]: * Camera Name → zed2
[ INFO] [1668675513.913827126]: * Camera Resolution → HD720
[ INFO] [1668675513.922761993]: * Camera Grab Framerate → 15
[ INFO] [1668675513.929198629]: * Gpu ID → -1
[ INFO] [1668675513.932919207]: * Camera ID → -1
[ INFO] [1668675513.936784139]: * Verbose → DISABLED
[ INFO] [1668675513.950123175]: * Camera Flip → DISABLED
[ INFO] [1668675513.960143748]: * Self calibration → ENABLED
[ INFO] [1668675513.970897192]: * Camera Model by param → zed2
[ INFO] [1668675513.971031721]: *** VIDEO PARAMETERS ***
[ INFO] [1668675513.978832209]: * Image resample factor → 0.5
[ INFO] [1668675513.988428362]: * Extrinsic param. frame → X RIGHT - Y DOWN - Z FWD
[ INFO] [1668675513.988590284]: *** DEPTH PARAMETERS ***
[ INFO] [1668675513.993451513]: * Depth quality → PERFORMANCE
[ INFO] [1668675513.997550687]: * Depth Sensing mode → STANDARD
[ INFO] [1668675514.010520919]: * OpenNI mode → DISABLED
[ INFO] [1668675514.021085817]: * Depth Stabilization → ENABLED
[ INFO] [1668675514.027541173]: * Minimum depth → 0.3 m
[ INFO] [1668675514.031441977]: * Maximum depth → 20 m
[ INFO] [1668675514.035670688]: * Depth resample factor → 0.5
[ INFO] [1668675514.036484104]: *** POSITIONAL TRACKING PARAMETERS ***
[ INFO] [1668675514.048500343]: * Positional tracking → ENABLED
[ INFO] [1668675514.052068056]: * Path rate → 2 Hz
[ INFO] [1668675514.056462561]: * Path history size → 1
[ INFO] [1668675514.065932025]: * Odometry DB path → /home/user/.ros/zed_area_memory.area
[ INFO] [1668675514.073191676]: * Save Area Memory on closing → DISABLED
[ INFO] [1668675514.093261046]: * Area Memory → ENABLED
[ INFO] [1668675514.101826309]: * IMU Fusion → ENABLED
[ INFO] [1668675514.111014266]: * Floor alignment → DISABLED
[ INFO] [1668675514.120616371]: * Init Odometry with first valid pose data → ENABLED
[ INFO] [1668675514.129472837]: * Two D mode → DISABLED
[ INFO] [1668675514.140037863]: *** MAPPING PARAMETERS ***
[ INFO] [1668675514.149275901]: * Mapping → DISABLED
[ INFO] [1668675514.170356736]: * Clicked point topic → /clicked_point
[ INFO] [1668675514.170561474]: *** OBJECT DETECTION PARAMETERS ***
[ INFO] [1668675514.179702839]: * Object Detection → DISABLED
[ INFO] [1668675514.180101082]: *** SENSORS PARAMETERS ***
[ INFO] [1668675514.188794923]: * Sensors timestamp sync → DISABLED
[ INFO] [1668675514.192935761]: * Max sensors rate → 200
[ INFO] [1668675514.193206164]: *** SVO PARAMETERS ***
[ INFO] [1668675514.199971442]: * SVO input file: →
[ INFO] [1668675514.204294458]: * SVO REC compression → H265 (HEVC)
[ INFO] [1668675514.212629864]: *** COORDINATE FRAMES ***
[ INFO] [1668675514.249014585]: * map_frame → map
[ INFO] [1668675514.249167450]: * odometry_frame → odom
[ INFO] [1668675514.249231995]: * base_frame → base_link
[ INFO] [1668675514.249321340]: * camera_frame → zed2_camera_center
[ INFO] [1668675514.249425277]: * imu_link → zed2_imu_link
[ INFO] [1668675514.249512093]: * left_camera_frame → zed2_left_camera_frame
[ INFO] [1668675514.249576894]: * left_camera_optical_frame → zed2_left_camera_optical_frame
[ INFO] [1668675514.249675167]: * right_camera_frame → zed2_right_camera_frame
[ INFO] [1668675514.249732575]: * right_camera_optical_frame → zed2_right_camera_optical_frame
[ INFO] [1668675514.249848737]: * depth_frame → zed2_left_camera_frame
[ INFO] [1668675514.249955777]: * depth_optical_frame → zed2_left_camera_optical_frame
[ INFO] [1668675514.250050274]: * disparity_frame → zed2_left_camera_frame
[ INFO] [1668675514.250131523]: * disparity_optical_frame → zed2_left_camera_optical_frame
[ INFO] [1668675514.250188740]: * confidence_frame → zed2_left_camera_frame
[ INFO] [1668675514.250266628]: * confidence_optical_frame → zed2_left_camera_optical_frame
[ INFO] [1668675514.267695494]: * Broadcast odometry TF → ENABLED
[ INFO] [1668675514.274966793]: * Broadcast map pose TF → ENABLED
[ INFO] [1668675514.283350583]: * Broadcast IMU pose TF → ENABLED
[ INFO] [1668675514.283733498]: *** DYNAMIC PARAMETERS (Init. values) ***
[ INFO] [1668675514.292950928]: * [DYN] Depth confidence → 30
[ INFO] [1668675514.300159859]: * [DYN] Depth texture conf. → 100
[ INFO] [1668675514.304719741]: * [DYN] pub_frame_rate → 15 Hz
[ INFO] [1668675514.324424115]: * [DYN] point_cloud_freq → 10 Hz
[ INFO] [1668675514.330192329]: * [DYN] brightness → 4
[ INFO] [1668675514.334335087]: * [DYN] contrast → 4
[ INFO] [1668675514.340103653]: * [DYN] hue → 0
[ INFO] [1668675514.343828327]: * [DYN] saturation → 4
[ INFO] [1668675514.348761493]: * [DYN] sharpness → 4
[ INFO] [1668675514.353194750]: * [DYN] gamma → 8
[ INFO] [1668675514.358078699]: * [DYN] auto_exposure_gain → ENABLED
[ INFO] [1668675514.370693632]: * [DYN] auto_whitebalance → ENABLED
[ INFO] [1668675514.429506561]: * Camera coordinate system → Right HANDED Z UP and X FORWARD
[ INFO] [1668675514.430703052]: *** Opening ZED 2…
[ INFO] [1668675520.271187146]: ZED connection → CAMERA STREAM FAILED TO START
[ INFO] [1668675528.162282020]: ZED connection → CAMERA STREAM FAILED TO START

MY PYTHON SCRIPT BELOW

import rospy
import pyzed.sl as sl
import numpy as np
import time
from classes.class_people import people
from classes.class_robot import robot

#used for tracked data
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from visualization_msgs.msg import MarkerArray
from visualization_msgs.msg import Marker

zed = sl.Camera()

init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720
init_params.camera_fps = 60
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP # Use a right-handed Z-up coordinate system
init_params.coordinate_units = sl.UNIT.METER # Set units in meters
init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE#ULTRA
init_params.sdk_verbose = True

#Open the camera
err = zed.open(init_params)
if (err != sl.ERROR_CODE.SUCCESS):
print(“camera initialization failed”)
exit()
print(“camera initialization finished”)
#---------------------------------------------------------------------

#-------------configurating object detection initialization and runtime parameter---------------------#
#models can be found on zed-python-api/sl.pyx at master · stereolabs/zed-python-api · GitHub
obj_param = sl.ObjectDetectionParameters()
obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST
obj_param.image_sync = True
obj_param.enable_tracking = True
obj_param.enable_body_fitting = True
#If you want to have object tracking you need to enable positional tracking first
if obj_param.enable_tracking:
positional_tracking_param = sl.PositionalTrackingParameters()
initial_position = sl.Transform()
initial_translation = sl.Translation()
initial_translation.init_vector(0, 0, 0) # in meter
initial_position.set_translation(initial_translation)
positional_tracking_param.set_initial_world_transform(initial_position)
zed.enable_positional_tracking(positional_tracking_param)

print(“Object Detection: Loading Module…”)
err = zed.enable_object_detection(obj_param)
if err != sl.ERROR_CODE.SUCCESS:
print(repr(err))
zed.close()
exit(1)
print(“finished”)

obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
obj_runtime_param.detection_confidence_threshold = 40
#----------------------------------------------------------------------------------------------------#

#--------------------preparing zed objects--------------------------#
objects = sl.Objects() #preparing the zed objects variable
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD
#-------------------------------------------------------------------#

if name == ‘main’:

zed_pose = sl.Pose()
zed_pose_publisher = rospy.Publisher("/zed2_pose", Pose, queue_size=1)
zed_pose_data = Pose()

# initializing node
print("creating node...")
rospy.init_node("human_pose_publisher", anonymous=False)
markerarray_publisher = rospy.Publisher("/interpolated_history_position", MarkerArray, queue_size=1)
print("waiting for odom data")
odom = rospy.wait_for_message("/locobot/mobile_base/odom", Odometry, timeout=None)
rate = rospy.Rate(60)
print("odom is retrieved")
# creating list of persons
People = people()
Locobot = robot(odom)
markerid_to_delete, markerArray_update_status = [], False

final_markerarray = MarkerArray()

while not rospy.is_shutdown():
    # retrieving the camera pose
    state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
    py_translation = sl.Translation()
    tx = round(zed_pose.get_translation(py_translation).get()[0], 3)
    ty = round(zed_pose.get_translation(py_translation).get()[1], 3)
    tz = round(zed_pose.get_translation(py_translation).get()[2], 3)
    print("Translation: tx: {0}, ty:  {1}, tz:  {2}, timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp))
    py_orientation = sl.Orientation()
    ox = round(zed_pose.get_orientation(py_orientation).get()[0], 3)
    oy = round(zed_pose.get_orientation(py_orientation).get()[1], 3)
    oz = round(zed_pose.get_orientation(py_orientation).get()[2], 3)
    ow = round(zed_pose.get_orientation(py_orientation).get()[3], 3)
    print("Orientation: ox: {0}, oy:  {1}, oz: {2}, ow: {3}\n".format(ox, oy, oz, ow))
    cam_pose_translation = [tx,ty,tz]
    cam_pose_orientation = [ox,oy,oz,ow]
    zed_pose_data.position.x = cam_pose_translation[0]
    zed_pose_data.position.y = cam_pose_translation[1]
    zed_pose_data.position.z = cam_pose_translation[2]
    zed_pose_data.orientation.x = cam_pose_orientation[0]
    zed_pose_data.orientation.y = cam_pose_orientation[1]
    zed_pose_data.orientation.z = cam_pose_orientation[2]
    zed_pose_data.orientation.w = cam_pose_orientation[3]
    zed_pose_publisher.publish(zed_pose_data)


    t1 = time.time()

    odom = rospy.wait_for_message("/locobot/mobile_base/odom", Odometry, timeout=None)
    Locobot.update_interpolated_poses(odom)

    if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Grab new frames and detect objects
        returned_state = zed.retrieve_objects(objects, obj_runtime_param) #retrieve objects

        if (returned_state == sl.ERROR_CODE.SUCCESS and objects.is_new):
            obj_array = objects.object_list
            print(str(len(obj_array)) + " Person(s) detected\n")

            if (obj_param.enable_tracking) == True:
                # First add new points and remove the ones that are too old
                current_timestamp = time.time()
                #rospy.loginfo("generating people objects")
                People.generate_from_objects(objects, current_timestamp, cam_pose_translation, odom)
                markerid_to_delete, markerArray_update_status = People.prune_old_interpolated_points(current_timestamp)  # here we delete old entries of interpolated points
                #People.prune_old_interpolated_points(current_timestamp)  # here we delete old entries of interpolated points

    rospy.loginfo("publishing FINAL marker array message")
    final_markerarray.markers = [Locobot.marker] + People.markerArray.markers
    markerarray_publisher.publish(final_markerarray)
    print("length of final markerArray: ", len(final_markerarray.markers))
    # print(final_markerarray.markers)

    People.prune_old_marker(markerid_to_delete, markerArray_update_status)
    rate.sleep()  # wichtig! otherwise your program will not exit ctrl + c

    t2 = time.time()
    print("one loop takes ", t2 - t1, "seconds")
6

when i run ZED_Diagnostic while my python script is running, it gives the X on Camera Test. However everything is fine when i dont run my python script.