Skip to content

RPi-CM4: Add Lucas-Kanade-Tomasi optical flow library and integrate with flight controller - #62

Draft
snktshrma wants to merge 3 commits into
ArduPilot:masterfrom
snktshrma:optical-flow
Draft

snktshrma wants to merge 3 commits into
ArduPilot:masterfrom
snktshrma:optical-flow

Conversation

@snktshrma

@snktshrma snktshrma commented Nov 5, 2026 •

Copy link
Copy Markdown
Member

This adds optical flow (KLT) into camera app for the RPi CM4 gimbal cam. The base algorithm and steps are in reference to BlueOS Optical Flow code.

Notes

  • Not yet tested on hardware. It builds on RPi os Bookworm and Trixie.

  • Tests and docs will follow in a separate commit (still completing).

  • The build currently depends on openCV (libopencv-dev), so this is CM4/RPi only for now....

  • I'm working on removing the openCV dependency, either with a dependency free C++ optical flow algo based on Birchfield's KLT tracker for AP_CameraGimbal or with the PX4-OpticalFlow SAD block matching algorithm. It will replace the openCV engine, either in this PR or in a follow-up in the next few days. That will make code compatible with other targets as well (based on what Tridge/Randy wants).

  • Some further cleanup is in progress.

  • A SITL based test in progress

Disclaimer: parts of this code were developed with the assistance of Claude : )

@snktshrma snktshrma changed the title RPi-CM4: Add Lucas-Kanade optical flow library and integrate with flight controller RPi-CM4: Add Lucas-Kanade-Tomasi optical flow library and integrate with flight controller Nov 5, 2026
@rmackay9

rmackay9 commented Nov 5, 2026

Copy link
Copy Markdown
Contributor

Sanket will kindly test this on a desktop version of my aerial photography copter. Thanks!

@AP-Review

AP-Review commented Nov 5, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-05)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: REQUEST CHANGES

Reviewed at head 8a4fbddc78.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ap_cameragimbal/62/1.html#prAP_CameraGimbal-62

Optical flow is cleanly integrated, but the flow vector ignores gimbal yaw, so it needs a fix before it is safe to feed to the EKF.

Blocking

  1. camera_app/src/opticalflow/optical_flow.cpp:85 Gimbal yaw is dropped: only pitch is kept, the down-facing check passes at any yaw (the CADDX allows -180 to 180 and yaw follows the vehicle), and image-axis rates go out as body-frame flow, so a 90 degree pan swaps the axes and 180 reverses them. FLOW_ORIENT_YAW is static and cannot compensate. Rotate the rates and pixel flow by the gimbal yaw, or withhold flow unless yaw is near zero.

Should fix

Low

Minor (4)
  • camera_app/src/opticalflow/optical_flow.cpp:166 A sample computed under the old config can still be polled after configure() disables flow (narrow window, one sample); recheck the generation in publish().
  • camera_app/src/opticalflow/optical_flow.cpp:159 The strict dt < 0.2 check rejects every frame at the supported 5 fps; allow a small tolerance.
  • camera_app/Makefile:484 make -C camera_app test now needs libopencv-dev on the host; list it in the host build prerequisites.
  • Tests: nothing exercises the LK engine, worker gating or OPTICAL_FLOW encoding; stub media has no flow_frame(), so the RPi host test never opens flow.

Withdrawn: the note that the first commit is not self-contained (no build compiles the wrapper at that commit, so bisect is unaffected).

@tridge

tridge commented Nov 6, 2026

Copy link
Copy Markdown
Contributor

@snktshrma thanks! it would be good for this to work in SITL too if we can for testing

@rmackay9

Copy link
Copy Markdown
Contributor

I've lightly tested this on the bench and it does produce OPTICAL_FLOW messages and they seem correct (I think)

moving right: positive X
moving left : negative X
moving forward: positive Y
moving backwards: negative Y

I hope to test this in the near future on a real vehicle

@AP-Review

Copy link
Copy Markdown

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: REQUEST CHANGES

Reviewed at head 64133b611e.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ap_cameragimbal/62/2.html#prAP_CameraGimbal-62

The gimbal-yaw blocker from the last review is still open, so this still needs changes before flow is safe to feed to the EKF.
Nothing from the previous review has been fixed yet: the PR was rebased and gained the web OpticalFlow tab, and the two flow commits are unchanged.

Blocking

  1. camera_app/src/opticalflow/optical_flow.cpp:85 Gimbal yaw ignored (still open).

Should fix

Low

Question

  • camera_app/src/opticalflow/optical_flow.cpp:163 @rmackay9's bench signs (right +X, forward +Y) don't both match ArduPilot's convention. At yaw 0 it expects moving the camera right to give negative flow_rate_x and forward to give positive flow_rate_y, and the code produces exactly that. Was it the camera or the scene that moved, and was flow_x or flow_rate_x read? A SITL or short hover check before flight would settle it.
Minor (5)

Withdrawn: the first-commit self-containment note stays withdrawn.

@rmackay9

Copy link
Copy Markdown
Contributor

I've bench tested this using my aerial photography copter and the horizontal directions are correct. Here's what the new optical flow page looks like:

image

@rmackay9

Copy link
Copy Markdown
Contributor

It might be good to remove the dependency on OpenCV so that it can be run on the Siyi A8 and other camera gimbals.

@rmackay9

Copy link
Copy Markdown
Contributor

I test flew this on a vehicle today and it worked very well even though it was very windy out. Logs available upon request!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants