Wrong Pitch during Flight

Software
1

This is a follow-up to our previous post (Orientation Error (Pitch, Roll) when Camera Straight Down) about Orientation errors during flight on a drone.

When using ZED SDK v3.7.2, the pitch and roll of the camera would get “stuck” during flight at 60,75, and 90 degrees, resulting in up to 50 degrees of error in the orientation.

When ZED SDK v3.8 released, we were hoping that the sl::Camera::setRegionOfInterest and/or PositionalTrackingParameters::depth_min_range would fix this as the one aspect that we failed to recognize from last time was that our drone’s landing gear was very visible (large black object in image below).

Left Landing Gear in frame
Left Landing Gear in frame1920×1080 91.3 KB

Using the ZED-GStreamer plugin (master), we set pos-depth-min-range=1.0 (landing gear are at maximum <0.5m away from the camera) and set the ROI to roi-x=700, roi-y=0, roi-w=520, roi-h=1080. While the camera’s orientation no longer gets “stuck” (the previous behavior), the pitch is correct until the camera begins facing “backwards”/past 90degrees (where 90 degrees is straight down, 0 degrees is horizon). Once it exceeds 90 degrees, it appears the returned pitch is true_value - 90 from what we can tell from our logs and video recording. For example, if the true pitch of the camera is 120 degrees, the orientation returned was 30 degrees (120 - 90 = 30).

We then did another test when holding the Drone/Camera by hand, tilting it past straight down, and the pitch was always correct. It appears the incorrect pitch behavior is only occurring during flight. At the time that this is occurring:

  • the camera is flying over/looking at a gravel road with grass surrounding and a trailer underneath it
  • is about 20ft above the ground
  • is measuring at least some depth values (indicated by our logs measuring the depth to the trailer)

We are running on:

  • Jetson Xavier NX
  • ZED-GStreamer (master)
  • ZED SDK 3.8.1
  • ZED 2i

Our (relevant) ZED GStreamer parameters:

  • stream-type: LEFT + DEPTH
  • camera-resolution: 1080p
  • camera-image-flip: OFF
  • set-as-static: TRUE
  • enable-positional-tracking: TRUE
  • enable-pose-smoothing: TRUE
  • enable-area-memory: TRUE
  • enable-imu-fusion: TRUE
  • coordinate-system: IMAGE
  • measure3D-reference-frame: WORLD
  • depth-mode: PERFORMANCE
  • sensing-mode: FILL
  • depth-minimum-distance: 300.0
  • depth-maximum-distance: 30000.0
  • depth-stabilization: TRUE
  • pos-depth-min-range: 1000.0
  • set-floor-as-origin: FALSE
  • set-gravity-as-origin: TRUE
  • roi: TRUE (roi-x: 700, roi-y: 0, roi-w: 520, roi-h: 1080)
  • initial-world-transform-x/y/z: 0
  • initial-world-transform-yaw: (custom)
  • aec-agc: TRUE (roi-x: 600, roi-y: 0, roi-w: 1320, roi-h: 1080, roi-side: LEFT)

(P.S. I did just find a bug in the implementation for ZED-GStreamer’s setRegionOfInterest that the safety checks for the ranges of roi-x/y/w/h are inverted. That was my fault as I did the PR for that update and will work on another to get that fixed (Bugfix for checking valid roi parameters by ryanppeters · Pull Request #45 · stereolabs/zed-gstreamer · GitHub). Regardless, the roi set using our parameters above actually passes the bad safety checks, and the roi is still set by the SDK)

1 Like
2

Hi @penguin
we noted this issue and reported it to the SDK team in order to eventually fix it.

3

@penguin can I ask you to modify the code to use the quaternion instead of the Euler angles to verify if the problem persists?

You must also modify the zed-meta accordingly…

Otherwise please open a feature request issue on Github and I try to make it the next week.

4

Thanks for the response.

I’ve done the quaternion changes on my local fork on the repo and we are looking to test/fly again tomorrow. I’ve added some simple logging to print the Euler Angles from ZED, quaternion from ZED, and quaternion from ZED converted to Euler using 3rd party library just to verify. I will post logs/screenshots of the results when finished.

In case the fix for this takes a while, does the SDK have the ability to provide orientation without visual odometry (ie just IMU)?

5

Hi @Myzhar, we finished up our flight testing today are here are our findings:

The problem still persists with quaternions. The behavior I described before was not entirely accurate: while the pitch is wrong once it is greater than 90deg, it was not a true_value - 90 error like I had initially thought, rather once the pitch is greater than ~60 degrees, it begins to become more and more incorrect.

Logs:
ZED_bad_pitch_during_flight.log (143.0 KB)

Each log entry corresponds to a GstZedSrcMeta from ZedSrc. Like you suggested, I altered the meta data to also include the quaternion along with the euler angles. Each log entry has the format:

[ROS Timestamp] <local_timestamp>
[ZED Quat] 	<quaternion from sl::Pose::getOrientation>
[ZED Euler (rad)] _____________  <ZED convert  quaternion -> euler (radians)>
[ZED ROS::TF Quat->Euler (rad)]  <ROS's tf package convert quaternion -> euler (radians)>
[ZED Euler (deg)] _____________ <ZED convert quaternion -> euler -> degrees>
[ZED ROS::TF Quat->Euler (deg)]  <ROS's tf package convert quaternion -> euler -> degrees>
[External IMU] ________________ <roll and pitch from noisy external IMU attached to camera>
[Interpolated] _________________ <ignore this>

ROS::TF
We attempted to use ROS’s tf library’s tf::Matri3x3(Quaternion).getRPY(float, float, float) to check ZED’s calculations but quickly realized that sl::Orientation.getRotationMatrix() (or something else) applied a frame transformation that we weren’t doing so that’s why they are sometime off by 180 - rotation. I would not focus too much on our ROS::TF logs.

External IMU:
Our external IMU is a Pololu MinIMU (accel + compass) but has a good bit of noise (+/- 2 deg) since the calibration software we use is not the best. Regardless, it gives a general ground truth for each log entry. The reason yaw is omitted is because the flight control used does a yaw recalibration mid-flight which causes offsets between it and the ZED camera, so it is better for us to use the flight controller yaw. You can see the flight controller yaw in the [Interpolated] log after being converted to the ENU frame

Log Overview:
I’ve truncated some of the logs and put visual breaks in the sections to help tell the story a little more in the file. I’ve left in all the logs once the pitch divergence/error begins in case it helps but only left in parts of the logs when it is correct. If you would like the full logs, I can provide. Here is a quick overview:

  1. Booted GStreamer pipeline with drone/camera on the ground, not moving. Orientations match
  2. 50 second pause after setting pipeline state to PAUSED and then drone takeoff
  3. Set GStreamer pipeline to play and first orientations are good (0 seconds into flight)
  4. 10 seconds into flight: Pitch is good
  5. 20 seconds into flight: Pitch is good
  6. 30 seconds into flight: Pitch is good
  7. 40 seconds into flight: Pitch is good
  8. 45 seconds into flight: Pitch begins to diverge from actual once pitch increases past 60 degrees
  9. 55 seconds into flight: Pitch fixes itself for about 0.5 seconds
  10. Pitch error gets worse until the camera pitches back to 60-70 degrees where it is correct again

It actually appears to be some of the same behavior as I saw on SDK v3.7.2 months ago, but it no longer gets “hard-stuck” at specifically 60, 75, and 90 degrees.

The pause that occurred during (2) does not appear to be the problem since the pitch is correct for 45 seconds after until the camera hits that ~60 degrees threshold. Our original testing with SDK v3.7.2 did not have this pause but still exhibited the incorrect pitch behavior.

Let me know if I can provide anything else to help you debug this

6

Thank you for the update and the useful information. I’m forwarding them to the positional tracking team.