AP_OpticalFlow - Add rover support #21510

stephendade · 2022-08-22T07:42:47Z

This PR adds optical flow support for Rover. No rangefinder is needed, as a new parameter (FLOW_HGT_OVR) is used to specify an "override" constant height above ground for the sensor.

Successfully tested with an UPixels UPFlow sensor, Mro X2.1-777 and https://www.servocity.com/bogie-runt-rover/

khancyr

LGTM

rmackay9 · 2022-08-22T10:32:39Z

This was my attempt at the same thing #20038

@stephendade I guess your implementation actually works? :-). Mine has a lot of code changes but it was only partially successful during actual testing (on a Copter).

geofrancis · 2022-08-22T10:43:02Z

very cool. does it have to track the ceiling? Would ground tracking work if it was in focus?

simon88 · 2022-08-22T14:39:09Z

NIce I'll try it, so you've tested with upwards hereflow and one upwards RF? A chance to port it to copter like @rmackay9 with #20038 ?

stephendade · 2022-08-23T23:48:52Z

@stephendade I guess your implementation actually works? :-). Mine has a lot of code changes but it was only partially successful during actual testing (on a Copter).

Yes, tested and does work nicely. This PR is simpler than your implemention - I only assume the sensor is upwards or downwards facing.

very cool. does it have to track the ceiling? Would ground tracking work if it was in focus?

Yes, ground tracking should work for Rover. Most sensors have a minimum focus distance of 10 or 20cm, so the chassis will need to be some distance off the ground.

NIce I'll try it, so you've tested with upwards hereflow and one upwards RF? A chance to port it to copter like @rmackay9 with #20038 ?

This does work with copter (though untested). The only sensor I've tested with is the UpFlow sensor, but it should work with all other supported sensors. Note that ArduPilot only supports having 1 optical flow sensor connected.

stephendade · 2022-08-23T23:49:56Z

Happy to assist anyone who wants to test this too!

EDIT:
During ceiling-tracking testing, I've noticed that the ceiling does need some texture/colour to be tracked properly. Most (plain white) ceilings won't give a good track. I'd suggest putting some high-contrast posters or shapes on the ceiling for better tracking - I used a bunch of AprilTags (A4 printouts, 1 per m^2) and got a decent track.

simon88 · 2022-08-31T08:01:07Z

Hi @stephendade
I've just tried to use your commit with copter, I suffered a crash just after take off. I had lot of EKF3 IMUX STOPPED AIDING and EKF3 IMUX STARTED RELATIVE AIDING. The copter take off and immediatly he went to the left, I had no control over the roll. Altitude was good. I also had some memory messages like can[125] exeptions : 88 free maybe a memoty issue?

My config Cube Orange HereFlow + 1 upwards LightWare lidar SF000/B on quadcopter.
here is the BIN in zip file 5 01-01-1980 01-00-00.zip
Just for information I add an another BIN file if you want to see my hereflow calibration with OF.flowX and OF.flowY 2 01-01-1980 01-00-00.zip (for me the calibration is good)

rishabsingh3003 · 2022-08-31T10:57:25Z

@stephendade I was trying to come up with something very similar a while back. As far as I can recall, when the EKF fuses the rangefinder data, it pretty much assumes that the orientation is downwards. It's hardcoded (I think) and it required a lot of changes to the EKF, so I gave up mid-way.
I don't see any changes to the EKF library in this PR.. I was wondering if my initial assessment was wrong?

I see those changes in @rmackay9 's PR.

stephendade · 2022-09-01T00:16:13Z

My config Cube Orange HereFlow + 1 upwards LightWare lidar SF000/B on quadcopter.

I don't see any changes to the EKF library in this PR.. I was wondering if my initial assessment was wrong?

@simon88 and @rishabsingh3003: There is no support for upward-facing rangefinders.

I suspect, since I've only been testing with rovers, any issues with height-above-ground (rangefinders) being fused with optical flow have been hidden. Obviously, rovers are always at 0m height-above-ground.

Unfortunately, I haven't got any copters to test with :(

What I'll likely do is limit the upwards-facing sensors to rover vehicles only.

rmackay9 · 2022-09-01T01:02:33Z

As @rishabsingh3003 mentioned, my PR #20038 modifies the EKF to use the appropriate range finder to match the orientation of the optical flow sensor.

So I think we could get this PR in if it only covered adding downward facing optical flow support to Rover. Once we try to add in the upward facing support then we need the EKF changes.

stephendade · 2022-09-01T04:29:58Z

My config Cube Orange HereFlow + 1 upwards LightWare lidar SF000/B on quadcopter.

I don't see any changes to the EKF library in this PR.. I was wondering if my initial assessment was wrong?

@simon88 and @rishabsingh3003: There is no support for upward-facing rangefinders.

I suspect, since I've only been testing with rovers, any issues with height-above-ground (rangefinders) being fused with optical flow have been hidden. Obviously, rovers are always at 0m height-above-ground.

Unfortunately, I haven't got any copters to test with :(

So I think we could get this PR in if it only covered adding downward facing optical flow support to Rover. Once we try to add in the upward facing support then we need the EKF changes.

Agree.

stephendade · 2022-09-07T06:57:48Z

I've updated this to remove the upwards facing support and have added a FLOW_HEIGHT parameter (only used in rover) to specify the height of the sensor above the ground.

It did require a number of changes in the AHRS/EKF to pass the height variable through correctly.

Initial tests were good.

rishabsingh3003

This looks better. However, I would imagine that since most optical flow sensors typically perform best when they are slightly high above the ground (at least 5-10 meters), the use case for this would be very limited. I think for Rover, it'll be much more useful to have an upward-facing optical flow (which I think now you have removed from this PR).
I would also imagine that this probably does not work for fast rovers, since the flow change will be too quick.

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

libraries/AP_NavEKF3/AP_NavEKF3_core.h

geofrancis · 2022-09-07T11:33:12Z

This looks better. However, I would imagine that since most optical flow sensors typically perform best when they are slightly high above the ground (at least 5-10 meters), the use case for this would be very limited. I think for Rover, it'll be much more useful to have an upward-facing optical flow (which I think now you have removed from this PR). I would also imagine that this probably does not work for fast rovers, since the flow change will be too quick.

where I think this will be very useful is tracking the ground outside on terrain where wheel slip is an issue so wheel encoders wont work. your correct that it would be speed limited but i suspect even 1m/s would be enough for most people when moving in a confined space. combined with proximity it should allow more precise navigation between obstacles without needing RTK.

looking at the px4flow specs show that it could do reasonable speeds just 1m above the ground, so i suspect it should work down to 10 or 20cm with some adjustments to focus.

Ground distance	1m	3m	10m
16mm lens	2.4m/s	7.2m/s	24m/s
8mm lens	4.8m/s	14.4m/s	48m/s
6mm lens	6.4m/s	19.2m/s	64m/s
4mm lens	9.6m/s	28.8m/s	96m/s

stephendade · 2022-09-09T08:34:04Z

looking at the px4flow specs show that it could do reasonable speeds just 1m above the ground, so i suspect it should work down to 10 or 20cm with some adjustments to focus.

I've got a UPFlow sensor mounted ~10cm above the ground. It's not great quality - I'll need to look at mounting it a bit higher for decent measurements.

This is mostly for indoor and/or slow vehicle, particularly non-GPS vehicles that need an EKF velocity source (and use computer vision for the position source).

geofrancis · 2022-09-23T08:10:13Z

I've got a UPFlow sensor mounted ~10cm above the ground. It's not great quality - I'll need to look at mounting it a bit higher for decent measurements.

have you tried adding a bright light at an angle similar to an optical mouse, it should give it more contrast and cast shadows on surface imperfections making them easier to track.

stephendade · 2022-09-30T07:19:50Z

@rishabsingh3003 and @rmackay9 this should be good to go. There appear to be a few unrelated tests failing though.

It's been tested with a rover and works fairly well!

libraries/AP_OpticalFlow/AP_OpticalFlow.h

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

tridge

need some checks by @priseborough

libraries/AP_DAL/AP_DAL.h

tridge · 2022-11-07T23:38:27Z

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -289,8 +289,12 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
    ftype vd = stateStruct.velocity.z;
    ftype pd = stateStruct.position.z;

-    // constrain height above ground to be above range measured on ground
+    // constrain height above ground to be above range measured on ground, or user specified height (if given, for rover)
+#if APM_BUILD_TYPE(APM_BUILD_Rover)


@priseborough why is this working? @stephendade reports he can get optical flow fusion with no rangefinder. Does this mean the EKF is not properly checking the health of the rangefinder before fusing flow data?

The optical flow fusion shouldn't be checking the range finder, it should be checking that the terrain height offsets state estimate is still valid. I had a quick look at the code and it does not appear that this is being done.

tridge · 2022-11-07T23:43:55Z

maybe we should not be dividing by DCM c.z ?

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

rmackay9 · 2022-11-08T11:28:04Z

As discussed on the dev call I tested disabling the rangefinder in Copter while using optical flow and it appears we have a bug (as @tridge suspected). I've raised an issue here.

stephendade · 2022-11-21T04:38:31Z

I've now fixed up the issues raised in the devcall by @rmackay9 and @tridge.

I will perform a few more tests with a real rover (once the weather is better) and then it should be good to go in.

stephendade · 2022-11-23T04:57:38Z

I've tested this on a real rover. Works pretty well.

I'm not great at EKF debugging, so happy to post other graphs as needed :)

Using GPS as Velocity source:

Using Optical flow as Velocity source:

tridge · 2022-11-28T23:16:37Z

testing copter flow in CIRCLE mode would be good, checking we get low innovations on flow and good pos hold in wind

priseborough

OK wth EKF changes but need to confirm that the height override cannot be set by other platforms.

stephendade · 2022-11-30T22:09:46Z

I've now tested this in copter SITL:

OK wth EKF changes but need to confirm that the height override cannot be set by other platforms.

Have confirmed that height override param can't be set in other platforms.

testing copter flow in CIRCLE mode would be good, checking we get low innovations on flow and good pos hold in wind

Circle mode still works well in windy (10m/s) conditions. Flow Innovations were the same vs master.

rmackay9 · 2022-11-30T23:03:57Z

Great stuff, merged, thanks!

stephendade added the Rover label Aug 22, 2022

khancyr approved these changes Aug 22, 2022

View reviewed changes

stephendade force-pushed the roverflow branch from 50de9ee to 6d6a863 Compare September 7, 2022 06:53

stephendade changed the title ~~AP_OpticalFlow - Add rover and upwards facing support~~ AP_OpticalFlow - Add rover a̶n̶d̶ ̶u̶p̶w̶a̶r̶d̶s̶ ̶f̶a̶c̶i̶n̶g̶ support Sep 7, 2022

rishabsingh3003 requested changes Sep 7, 2022

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp Outdated Show resolved Hide resolved

libraries/AP_NavEKF3/AP_NavEKF3_core.h Outdated Show resolved Hide resolved

stephendade force-pushed the roverflow branch from 6d6a863 to 856bfb5 Compare September 9, 2022 08:28

stephendade force-pushed the roverflow branch from 856bfb5 to 06818ab Compare September 9, 2022 09:36

stephendade force-pushed the roverflow branch 2 times, most recently from 752d7d1 to e7715de Compare September 23, 2022 06:56

stephendade force-pushed the roverflow branch 3 times, most recently from 909c7e6 to c0bd0f4 Compare September 30, 2022 05:41

stephendade force-pushed the roverflow branch from c0bd0f4 to ca9a8b8 Compare October 10, 2022 06:06

stephendade changed the title ~~AP_OpticalFlow - Add rover a̶n̶d̶ ̶u̶p̶w̶a̶r̶d̶s̶ ̶f̶a̶c̶i̶n̶g̶ support~~ AP_OpticalFlow - Add rover support Oct 10, 2022

stephendade added the DevCallTopic label Oct 10, 2022

rmackay9 reviewed Nov 7, 2022

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow.h Outdated Show resolved Hide resolved

rmackay9 reviewed Nov 7, 2022

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp Outdated Show resolved Hide resolved

tridge requested a review from priseborough November 7, 2022 23:39

tridge requested changes Nov 7, 2022

View reviewed changes

CraigElder removed the DevCallTopic label Nov 7, 2022

rmackay9 reviewed Nov 7, 2022

View reviewed changes

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp Outdated Show resolved Hide resolved

stephendade force-pushed the roverflow branch 2 times, most recently from 232f19b to 428a4e1 Compare November 14, 2022 04:28

stephendade added 6 commits November 21, 2022 08:32

Rover: Add optical flow support

666d7b6

AP_AHRS: Add support for fixed height optical flow

56d4215

AP_DAL: Add support for fixed height optical flow

fa9de80

AP_NavEKF2: Add support for fixed height optical flow

5858bcb

AP_NavEKF3: Add support for fixed height optical flow

f8ff214

AP_OpticalFlow: Add support for fixed height optical flow

66cc3b3

stephendade force-pushed the roverflow branch from 428a4e1 to 66cc3b3 Compare November 20, 2022 21:41

rmackay9 approved these changes Nov 22, 2022

View reviewed changes

stephendade added the DevCallTopic label Nov 28, 2022

tridge added the NeedsTesting label Nov 28, 2022

CraigElder removed the DevCallTopic label Nov 28, 2022

priseborough approved these changes Nov 28, 2022

View reviewed changes

tridge approved these changes Nov 28, 2022

View reviewed changes

rmackay9 merged commit e6b3028 into ArduPilot:master Nov 30, 2022

rmackay9 mentioned this pull request Nov 30, 2022

Rover: support optical flow #7420

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_OpticalFlow - Add rover support #21510

AP_OpticalFlow - Add rover support #21510

stephendade commented Aug 22, 2022 •

edited

Loading

khancyr left a comment

rmackay9 commented Aug 22, 2022

geofrancis commented Aug 22, 2022 •

edited

Loading

simon88 commented Aug 22, 2022 •

edited

Loading

stephendade commented Aug 23, 2022

stephendade commented Aug 23, 2022 •

edited

Loading

simon88 commented Aug 31, 2022 •

edited

Loading

rishabsingh3003 commented Aug 31, 2022

stephendade commented Sep 1, 2022

rmackay9 commented Sep 1, 2022 •

edited

Loading

stephendade commented Sep 1, 2022

stephendade commented Sep 7, 2022

rishabsingh3003 left a comment

geofrancis commented Sep 7, 2022 •

edited

Loading

stephendade commented Sep 9, 2022

geofrancis commented Sep 23, 2022

stephendade commented Sep 30, 2022

tridge left a comment

tridge Nov 7, 2022

priseborough Nov 10, 2022

tridge commented Nov 7, 2022

rmackay9 commented Nov 8, 2022

stephendade commented Nov 21, 2022

stephendade commented Nov 23, 2022

tridge commented Nov 28, 2022

priseborough left a comment

stephendade commented Nov 30, 2022

rmackay9 commented Nov 30, 2022

AP_OpticalFlow - Add rover support #21510

AP_OpticalFlow - Add rover support #21510

Conversation

stephendade commented Aug 22, 2022 • edited Loading

khancyr left a comment

Choose a reason for hiding this comment

rmackay9 commented Aug 22, 2022

geofrancis commented Aug 22, 2022 • edited Loading

simon88 commented Aug 22, 2022 • edited Loading

stephendade commented Aug 23, 2022

stephendade commented Aug 23, 2022 • edited Loading

simon88 commented Aug 31, 2022 • edited Loading

rishabsingh3003 commented Aug 31, 2022

stephendade commented Sep 1, 2022

rmackay9 commented Sep 1, 2022 • edited Loading

stephendade commented Sep 1, 2022

stephendade commented Sep 7, 2022

rishabsingh3003 left a comment

Choose a reason for hiding this comment

geofrancis commented Sep 7, 2022 • edited Loading

stephendade commented Sep 9, 2022

geofrancis commented Sep 23, 2022

stephendade commented Sep 30, 2022

tridge left a comment

Choose a reason for hiding this comment

tridge Nov 7, 2022

Choose a reason for hiding this comment

priseborough Nov 10, 2022

Choose a reason for hiding this comment

tridge commented Nov 7, 2022

rmackay9 commented Nov 8, 2022

stephendade commented Nov 21, 2022

stephendade commented Nov 23, 2022

tridge commented Nov 28, 2022

priseborough left a comment

Choose a reason for hiding this comment

stephendade commented Nov 30, 2022

rmackay9 commented Nov 30, 2022

stephendade commented Aug 22, 2022 •

edited

Loading

geofrancis commented Aug 22, 2022 •

edited

Loading

simon88 commented Aug 22, 2022 •

edited

Loading

stephendade commented Aug 23, 2022 •

edited

Loading

simon88 commented Aug 31, 2022 •

edited

Loading

rmackay9 commented Sep 1, 2022 •

edited

Loading

geofrancis commented Sep 7, 2022 •

edited

Loading