Sophus ensure failed

[boardd]

Preliminary Checks

• This issue is not a duplicate. Before opening a new issue, please search existing issues.
• This issue is not a question, feature request, or anything other than a bug report directly related to this project.

Description

When playing back svo2 and doing positional tracking GEN2, it works like normal for a while and then I get this error. This only happens to some svo2 files, but I have no idea what causes it. This is a problem because I am using the zed for data collection, so I need to be able to reliably run the positional tracking offline.

Link to svo file: https://drive.google.com/file/d/1y0ktUFY-hM0JCA84siHiju32R7aO7TsW/view?usp=sharing

Sophus ensure failed in function 'static IOFTRACKER_Sophus::SO3<Scalar_> IOFTRACKER_Sophus::SO3<Scalar_, Options>::expAndTheta(const Tangent&, IOFTRACKER_Sophus::SO3<Scalar_, Options>::Scalar*) [with Scalar_ = float; int Options = 0; IOFTRACKER_Sophus::SO3<Scalar_, Options>::Tangent = Eigen::Matrix<float, 3, 1>; IOFTRACKER_Sophus::SO3<Scalar_, Options>::Scalar = float]', file '/builds/sl/ZEDKit/lib/include/sl_core/tracking/Tracker/IOFTracker/sophus/so3.hpp', line 612.

Steps to Reproduce

1. Load SVO2 file
2. Enable Positional Tracking
3. Get positions and grab camera frames
4. Error appears out of nowhere
   ...

Expected Result

Positional Tracking continues and finishes when svo runs out.

Actual Result

The program aborts

ZED Camera model

ZED Mini

Environment

Zed SDK 4.2 (using the python SDK)
Python Version = Python 3.10.12
Ubuntu 22.04

Anything else?

My function:

def process_svo_gen2(file_path, all_data, use_viewer = False, show_image = False, skip_slam = False, use_imu = True):
    # Create a ZED camera object
    zed = sl.Camera()

    # Initialize the parameters for opening the SVO file
    init_params = sl.InitParameters()
    init_params.set_from_svo_file(file_path)
    init_params.svo_real_time_mode = False  # Disable real-time mode for better processing
    init_params.coordinate_units = sl.UNIT.METER
    init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP # Set a coordinate system for tracking
    init_params.depth_mode = sl.DEPTH_MODE.ULTRA

    
    # Open the SVO file
    status = zed.open(init_params)
    if status != sl.ERROR_CODE.SUCCESS:
        print(f"Failed to open SVO file: {status}")
        return

    if not skip_slam:
        # Enable positional tracking
        tracking_params = sl.PositionalTrackingParameters()
        tracking_params.set_gravity_as_origin = False
        tracking_params.enable_area_memory = True
        tracking_params.enable_pose_smoothing = False
        tracking_params.enable_imu_fusion = use_imu
        tracking_params.mode = sl.POSITIONAL_TRACKING_MODE.GEN_2
        # tracking_params.area_file_path = "test.area"
        
        status = zed.enable_positional_tracking(tracking_params)
        
        if status != sl.ERROR_CODE.SUCCESS:
            print(f"Failed to enable positional tracking: {status}")
            zed.close()
            return
    
    all_episodes = sorted(all_data.keys())
    
    for ep in all_episodes:
        all_data[ep]["imu_data"] = []
        all_data[ep]["tracked_pose"] = []
        all_data[ep]["trans_pose"] = [] 
        all_data[ep]["SLAM_failed"] = False
    
    curr_ep = all_episodes[0] # get the first episode in the list
    
    if use_viewer:
        camera_info = zed.get_camera_information()
        viewer = gl.GLViewer()
        viewer.init(camera_info.camera_model)
        
    left_image = sl.Mat()
    image_res = sl.Resolution(320, 240)
    
    fail_count = 0
    
    # Loop through frames in the SVO file
    while not use_viewer or viewer.is_available():
        zed_pose = sl.Pose()
        
        if zed.grab() == sl.ERROR_CODE.SUCCESS:
            # Get the pose of the camera relative to the world frame
            timestamp = zed.get_timestamp(sl.TIME_REFERENCE.IMAGE).get_nanoseconds()
            
            if use_imu:
                sensor_data = sl.SensorsData()
                if (zed.get_sensors_data(sensor_data, sl.TIME_REFERENCE.IMAGE) == sl.ERROR_CODE.SUCCESS):
                    linacc = sensor_data.get_imu_data().get_linear_acceleration() # meters per second squared
                    angvel = sensor_data.get_imu_data().get_angular_velocity() # degrees per second
                    
                    lx = linacc[0]
                    ly = linacc[1]
                    lz = linacc[2]
                    
                    # convert degrees to radians
                    ax = angvel[0] * np.pi / 180
                    ay = angvel[1] * np.pi / 180
                    az = angvel[2] * np.pi / 180
                    
                    all_data[curr_ep]["imu_data"].append([timestamp, lx, ly, lz, ax, ay, az])
                    
            if not skip_slam:
                state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)
                tracking_status = zed.get_positional_tracking_status()
            
            zed.retrieve_image(left_image, view = sl.VIEW.LEFT, resolution = image_res)
            
            img = left_image.get_data()
            
            if show_image:
                cv2.imshow("Image", img)
                key = cv2.waitKey(10)
            
            if not skip_slam and state == sl.POSITIONAL_TRACKING_STATE.OK:
                # translation_left_to_center = zed.get_camera_information().camera_configuration.calibration_parameters.get_camera_baseline() / 2
                # Display translation and timestamp
                            
                py_translation = sl.Translation()
                tx = zed_pose.get_translation(py_translation).get()[0]
                ty = zed_pose.get_translation(py_translation).get()[1]
                tz = zed_pose.get_translation(py_translation).get()[2]
                
                #Display orientation quaternion
                py_orientation = sl.Orientation()
                ox = zed_pose.get_orientation(py_orientation).get()[0]
                oy = zed_pose.get_orientation(py_orientation).get()[1]
                oz = zed_pose.get_orientation(py_orientation).get()[2]
                ow = zed_pose.get_orientation(py_orientation).get()[3]
                
                if use_viewer:
                    rotation = zed_pose.get_rotation_vector()
                    translation = zed_pose.get_translation(py_translation)
                    text_rotation = str((round(rotation[0], 2), round(rotation[1], 2), round(rotation[2], 2)))
                    text_translation = str((round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2)))

                    viewer.updateData(zed_pose.pose_data(sl.Transform()), text_translation, text_rotation, tracking_status)
                ```

[TanguyHardelin]

Hello @boardd,

Thank you for your feedback. I have tested your SVO2 on the provided code snippet but have been unable to reproduce the issue. To assist further, could you please provide additional details about your configuration setup, such as your CPU/GPU specifications and whether you are running the program independently or concurrently with other processes?

While we have addressed many issues in the upcoming version that could potentially be related to your problem, this particular issue has not been encountered before. It is important for us to reproduce it to determine if it has been resolved with our recent fixes. Kindly share any pertinent information necessary for replicating the error, especially details about your configuration setup and system, as we suspect a synchronization issue may be at play.

Regards,
Tanguy

[boardd]

Hi @TanguyHardelin. Yeah of course.

CPU: 12th Gen Intel® Core™ i7-12800H
GPU: NVIDIA GeForce RTX 3080

I'm not sure what you mean by my configuration setup. Could you be more specific about what information is missing?

[boardd]

After some further debugging, the issue seems to happen more when I have init_params.svo_real_time_mode = False. When it is set to true, I think the issue happens less frequently

Can you explain what the difference between real time mode and non real time mode? Not sure I understand why that might be causing it.