Fusion.geo_to_camera() does not update pose

Software
1

I am attempting to ameliorate some of the current SDK quirks with my own functionality. This solution is meant as a workaround to the missing GeoPose.heading value, by way computing the local coordinate systems origin yaw geographical heading:

    def solve_local_cs(self):
        """
        Solve for the local coordinate system's geographic heading based on the camera's pose and the global reference vectors.

        Returns:
            None
        """
        origin_unit_up = np.array([0,1,0]).reshape(-1,1)

        unit_val = 100 # meters
        origin_geopose = sl.GeoPose()
        origin_pose = sl.Pose()
        status:sl.GNSS_FUSION_STATUS = self.fusion.camera_to_geo(origin_pose,origin_geopose)

        # Get global reference vectors
        origin_latlng: sl.LatLng = origin_geopose.latlng_coordinates

        lat,lon,elev = origin_latlng.get_coordinates(False)

        new_lat,new_lon = increase_latitude(lat=lat,lon=lon,elevation=elev,distance_meters=unit_val)

        global_up_new = sl.LatLng()
        global_up_new.set_coordinates(new_lat,new_lon,elev,False)

        assert int(np.round(np.abs(sl.GeoConverter.latlng2utm(global_up_new).northing - sl.GeoConverter.latlng2utm(origin_latlng).northing))) == unit_val, "Coordinate transformation did not work"


        global_up_pose = sl.Pose()
        status:sl.GNSS_FUSION_STATUS = self.fusion.geo_to_camera(global_up_new,global_up_pose)

        geo_unit_up: np.ndarray = global_up_pose.get_translation().get().reshape(-1,1)

        if np.sum(np.abs(geo_unit_up)) > 0:
            geo_unit_up /= np.linalg.norm(geo_unit_up)
            # Solve for rotation matrix
            # R@X_geo_unit_up = X_local_unit_up
            # R = (X_l@X_l')@(X_g@X_l')-1
            # C = A@B^-1
            A = origin_unit_up@origin_unit_up.T
            B = geo_unit_up@origin_unit_up.T
            rot_mat = A@np.linalg.inv(B)

            # Get theta
            theta:float = np.rad2deg(np.arcsin(rot_mat[0][1]))


            self.origin_geoheading = theta

The issue is that despite getting a good value from self.fusion.camera_to_geo(),
it seems that the conjugate function selffusion.geo_to_camera() does not currently work. Is there anything I can do to get the expected output for fusion.geo_to_camera()?

2

Hi @fdunbar,

Thank you for reporting this, we will investigate whether there is indeed an issue with the fusion.get_geo_to_camera method.

3

Hello @fdunbar,
I have reviewed your issue, and I wasn’t able to reproduce it. I’d like to point out that the heading feature you mentioned was added in release_4.1. If you haven’t updated to that version, it might explain why you’re not seeing it.

Additionally, I would like to clarify that the GeoPose lat/long coordinate represents only the translation part of the fused position projected into the GEO_WORLD reference. When you pass through this coordinate, you lose the rotation part of your fused position. As a result, it becomes impossible to retrieve the heading information.

I hope this help.
Regards