Barometric factor

[PeterQFR]

Add a factor that handles barometric pressure readings as a constraint on the z height of a Pose3 value.
The conversion from pressure to height is given by conversions from https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html

The model is

measured height in meters = Pose.z() + bias + noise;

With Pose and Bias both being values in the graph.

There are conversions both ways between pressure and height in meters.

We are only handling the tropospheric conversion from 0 to 11km.

I have to remove CMakelists changes for installation in a Ament environment.

[dellaert]

Cool! Could you:

• format with Google style, e.g. cppformat in vs code.
• Add a test with non-zero, non-identity poses
• un comment or remove other test?
  Then please request a review by me.

[PeterQFR]

I've put in the non-zero tests, however I get an error with the numerical differentiation for the jacobian. Whenever there is any roll or pitch (Rx Ry) applied to the pose, the numerical derivative seems to use that to calculate the change in z in the local frame.

I believe the analytic jacobians are correct in the evaluateError() but I would not be suprised if they are not. so would like some help deriving them if they are not correct.

Ie at the moment, for the pose3 only Tz contributes anything to the error so the jacobian should only be dependent on it (and the bias).

[PeterQFR]

@dellaert I've applied the formatting and non-zero tests, but need some help with the Jacobians I am returning for the Pose3 value.

It does not agree with the numerical derivative in the test when Rx or Ry is applied to the Pose3d. It is like the numerical derivative is calculated in the child frame orientation of the pose where as I used to the translation being with respect to the parent frame's orientation. The analytical jacobian of the error I think is correct. Have I introduced some error in the numerical differentiation?

[dellaert]

Yes, the frame in which you calculate Jacobians is typically the source of many problems. It has to be done in the local frame. The translation method will do this for you, and does not agree with the intuition of 000001.

try that?

[ProfFan]

BTW @dellaert do we need a new subfolder for this factor?

[dellaert]

BTW @dellaert do we need a new subfolder for this factor?

No, navigation is fine. Will merge now.

[dellaert]

Looks awesome! Many thanks for your contribution!

[mnissov]

Doesn't this also need an implementation for the bias dynamics? As far as I can see this factor has no consideration of the bias random walk, which I think is necessary for this to be used?

[PeterQFR]

Thats right, but that is common with a lot of factor implementations. Here the random walk is achieved by adding additional between factor between bias values at each measurement step.
That is, you need to add an additional Between factor, linking each of your Bias Values in the graph. Its not included in this factor because that would mean this factor would then need to reference the bias of the previous timestep.
This factor doesn't assume anything about the bias which could be constant among other things.