Nearest neighbor search for 8 points on 2 z planes as inputs for interpolation

[mabelzhang]

Depends on #70. PR on trilinear interpolation to follow.
Anyone free can review. This PR is pretty independent, mainly matrix algorithm.

Two substantial functions are added. To keep PRs easier to review, I thought this should go in before the actual trilinear interpolation in a separate PR.

What this PR does, from major to minor:

• Two new functions, FindInterpolators() and SearchInDepthSlice(), compute the inputs to go into trilinear interpolation (see algorithm below)
• Convert sensor location from lat/long to Cartesian (this is what depends on Science data: lat/lon to cartesian #70, for the conversion function, and for the converted sensor position to make sense with the science data, which were already converted)
• Fix a seg fault I previously had in this branch in the private repo
• Decrease the number of times sensor location is updated and interpolation is called. These clog up the pipeline I think, causing science visualization to never show up. Interpolation doesn't need to run for every sensor in every PostUpdate(). It really only needs to happen when the robot has moved far enough to warrant a sensor location update, and only once for the entire vehicle (as opposed to every sensor).

Algorithm for 8-neighbors search for interpolation

The inputs to trilinear interpolation are 8 conceptual "nearest neighbors," but not precisely NNs, because they are required to be on 2 distinct z slices, in order to be useful for trilinear interpolation.

To find the 8 NNs, I cannot just look for 2 NNs directly, because they just ended up on the same z slice and were useless for interpolation.

This is the current algorithm. It makes liberal use of the raw Eigen::MatrixXf underlying pcl::PointCloud:

1. Find 1st NN.
2. Create a z slice of all points in cloud matching the z of the 1st NN
3. In this z slice, find 4 NNs. This is the 1st set.
4. Remove all points in the z slice of the 1st NN from future searches
5. In the remainder of the cloud, find 2nd NN
6. Create a second z slice for all points matching z of 2nd NN
7. In this z slice, find 4 NNs. This is the 2nd set.
8. Return the 2 sets of 4 NNs, each set being on a different z slice.

To test

You should see output similar to this at startup:

$ ign gazebo -v 4 buoyant_tethys.sdf
...
[Dbg] [ScienceSensorsSystem.cc:891] Number of points matching z == -0: 18471
[Dbg] [ScienceSensorsSystem.cc:947] Found 4 neighbors in this slice.
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-8.3819e-13, -4.65661e-14, -0), distance 1.01812e-05 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-0.0954054, -4.65661e-14, -0), distance 0.0954054 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (0.0954592, -4.65661e-14, -0), distance 0.0954592 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-0.0142803, -0.180422, -0), distance 0.180986 m
[Dbg] [ScienceSensorsSystem.cc:850] Slice 2 nnIdx 4021, dist 4.99999
[Dbg] [ScienceSensorsSystem.cc:891] Number of points matching z == -5: 18471
[Dbg] [ScienceSensorsSystem.cc:947] Found 4 neighbors in this slice.
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-8.3819e-13, -4.65661e-14, -5), distance 4.99999 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-0.0954054, -4.65661e-14, -5), distance 5.0009 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (0.0954592, -4.65661e-14, -5), distance 5.0009 m
[Dbg] [ScienceSensorsSystem.cc:955] Neighbor at (-0.0142803, -0.180422, -5), distance 5.00326 m

Significance is that the 2 slices need to have different z's. Otherwise they're useless.

You should also verify that science data visualization still works - follow steps in #70.
The reason is that, if FindInterpolators() is called too often (every PostUpdate() multiple times), visualization never shows up.
I added a threshold INTERPOLATE_DIST_THRESH of 50 meters for the robot to move, before that function is called.
That threshold will need to be decrease when we actually test, because we currently downscale the data by a LOT to work around the orbiting tools.

Future work

To test these thoroughly, we still need

• Actual trilinear interpolation, in a followup PR
• There needs to be a check for a corner case where the robot is at the boundary of a cluster of data, and its two closest z slices are both below (or both above) the robot, instead of one above and below. The next one above might not be for a very long way. The current algorithm would return both z slices below, incorrect for interpolation.

Then we can test statically and/or dynamically, which need

• Statically by spawning vehicle at different lat/long. Prerequisite: All science data need to be shifted to the new lat/long that the world origin is associated with, when vehicles are spawned using empty_environment.sdf. Currently, this is not implemented yet (see stub function ShiftDataToNewSphericalOrigin().
• (Might be able to test statically by hardcoding a different vehicle XYZ in the SDF?)
• Dynamically. Move vehicle enough distance to test sensor data changing (acceptance mission probably does this for us)

[tfoote]

We shouldn't use this magic number. It should be related to the data density. The optimal would be to compute the bounding box of when a nearest neighbor will transition. However as a proxy some fraction of the distance to the nearest neighbor might make sense as a heuristic causing over computation but it will be data driven instead of a magic number.

[mabelzhang]

This is just the quickest way to cut down the number of times interpolation is run, so that it doesn't run every PostUpdate(). This code is not tested until we have interpolation, so this is more a preventative measure.

We can add optimization later, if speed is a problem. That requires some thought, because the data has variable density. That will be a PR in itself.

[mabelzhang]

I should add that the only reason this was introduced right now, is that none of the visualization was showing when interpolation was always called.

[mabelzhang]

I just merged from main. We made a lot of changes in 2 other PRs and discarded the branch this PR was originally targeting, so it was more like a rebase. Git auto-merge made some mistakes.
There were a lot of conflicts because of indentation diffs from #98 interjected in between code blocks added to PostUpdate() in this PR. Hopefully there were no merge errors.

Coordinate system has been changed in other PRs. So the sample output looks different now. Still just paying attention to depths being 0 and 5 though:

[Dbg] [ScienceSensorsSystem.cc:583] At time slice 0, populated 92355 spatial coordinates.
[Dbg] [ScienceSensorsSystem.cc:1080] Searching around sensor Cartesian location 8.83035e-39, -2.65023e-13, -1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:691] 92355 points
[Dbg] [ScienceSensorsSystem.cc:693] Slice 1 nnIdx 4021, dist 1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:807] Number of points matching z == -0: 18471
[Dbg] [ScienceSensorsSystem.cc:856] Found 4 neighbors in this slice.
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (1.39148e-12, -1.1001e-13, -0), distance 1.01813e-05 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (-0.181203, 1.84071e-05, -0), distance 0.181203 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (0.181203, 1.84071e-05, -0), distance 0.181203 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (1.37634e-12, -0.221866, -0), distance 0.221866 m
[Dbg] [ScienceSensorsSystem.cc:766] Slice 2 nnIdx 4021, dist 4.99999
[Dbg] [ScienceSensorsSystem.cc:807] Number of points matching z == -5: 18471
[Dbg] [ScienceSensorsSystem.cc:856] Found 4 neighbors in this slice.
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (1.39148e-12, -1.1001e-13, -5), distance 4.99999 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (-0.181203, 1.84071e-05, -5), distance 5.00327 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (0.181203, 1.84071e-05, -5), distance 5.00327 m
[Dbg] [ScienceSensorsSystem.cc:864] Neighbor at (1.37634e-12, -0.221866, -5), distance 5.00491 m

[mabelzhang]

Ready for another pass.

This is one of the most time-sensitive pieces for milestone 4.
I would appreciate help to get these interpolation PRs in ASAP, so that there are fewer conflicts to resolve.

In the interest of time, we should focus on critical functionality, and leave less critical improvements to a future PR, opening issues if necessary. Reminder that code in here is by no means finalized. Multiple steps are still to come. This is the 1st of >=2 PRs for interpolation, to keep PRs small and easier to review. Some of the API here are already changed in my new branch to be the 2nd PR. So we're not looking for perfection here, just for algorithmic correctness.

[mabelzhang]

Might close if #110 is a good alternative.
On hold until that is clear after thorough tests some time later. Not urgent anymore for Q4.

[tfoote]

I see you commented that this is on hold pending the replacement in #110 which has an alternative implementation. I'll focus over there.

[mabelzhang]

In the meeting today, we verbally agreed that this is still needed, because I remembered why we needed trilinear interpolation in the first place while debugging in #126 (review) . Basically, the data we're working with does not lend itself to simple kNN searches, because we simply get a vertical stick in z, since z resolution is on the scale of a few meters, when x and y are on the scale of hundreds of meters or km. So we have to do the 2 separate z slices, which this PR does.

I have a hybrid approach using both methods that might work. That is in progress in the mabelzhang/trilinear_interp branch that's to come after this PR.

In this PR, I merged from main in 0af6a57. Since main already has barycentric interpolation, there were a bunch of conflicts and a big diff, but none of the actual algorithm was changed. Some of the function signatures had to be changed. There's a new Boolean to choose between barycentric and trilinear interpolations. The dummy TrilinearInterpolate() that just returns the closest point had to be updated to work with the new API. It doesn't matter too much - it's already replaced in mabelzhang/trilinear_interp with the actual interpolation.

[tfoote]

I have a few changes and comments inline here. And the search algorithm I wanted to suggest refactoring it to simply do two slices and then find the nearest neighbors. I also found some magic numbers and removed some unnecessary arguments.

I've put it forth in #142

Also looking at the CI logs we're generating a lot of debug info when the data isn't present. We might want to make things less verbose, and not try as hard if there's no data in the world.

[tfoote]

What should the "error" response be expected. In the implementation it will simply return if the preconditions are unmet. How should a user know that the search failed? Do they have to check the length of the output parameters?

I might suggest a return code with success code. Otherwise the implementation should at least be emptying the output vectors so they can be checked for length. I'm not familiar with the coding style my other suggestion would be an exception but I haven't seen that in use here.

[mabelzhang]

Yeah, they should be cleared. feb33ed
We don't have standard success codes, at least in this project. I haven't seen them much in ign-gazebo plugins, because most plugin functions are in a private class internal to individual plugins. They may be called from only one place ever (as is the case here), so it doesn't need to be so formal.

[tfoote]

These are magic numbers and should be parameters to the search and passed down. (Note that these are very tight tolerances. Expecting micrometer precision in a dataset seems unrealistic to me. I would expect this to be something like looking for things on the scale of the search that we're performing.)

[mabelzhang]

1e-6 is actually a very standard tolerance used in a lot of places in Ignition, to do an a == b equivalence check while catching floating point errors. Lines 859-860 essentially check for z == _depth. It does this because the numbers in the data are quite perfect, as data generated from models would be. The floating point check is there more for a memory representation thing, than for the user data being off.

The problem of setting tolerance ranges other than a == b, is that we don't know the resolution of the data, and we know that the data resolution varies by location. So there isn't one number that would work for all the data in a single data set. The resolution pretty much has to be calculated all the time, which can be a performance drain.

[tfoote]

There's a potential debate about what the range should be. But it definitely should not be a magic number in the code here. It should be passed in at a higher level such that it has potential to be adjusted for the different dataset if required.

[mabelzhang]

I see that you put an alternative in #142. Can we break just the part about this comment off into its own PR? See #142 (comment) I would really like to base proposals for big changes on #127, where the interpolation algorithms are implemented, because even if the search in #83 gives us some results, we now know that it doesn't mean those neighbors will work with interpolation, because of the constraints and edge cases in the interpolation algorithms.

[tfoote]

This negative slice logic is not necessary. You have the original cloud and the indices of the zSliceInds1.

The excluded cloud is all points with other indices. You don't have to redo the search for points outside of the z1 plane.

[mabelzhang]

That saves performance, but it does add more code. f3f72c2

We need a ExtractIndices filter to get the inverted sub-cloud. It's less clean to read, because ExtractIndices::setIndices() only accepts PCL IndicesPtr type. It doesn't accept the std::vector returned from PassThrough. So we have to convert the type first.

I tested the new replacement with the 3 csv files in buoyant_tethys.sdf and verified the outputs look the same before and after this change. But again, given the FIXMEs, the outputs from this PR are not correct, so I would not trust things beyond small mechanical changes.

[mabelzhang]

I addressed the comments individually here. Please see my comment here #142 (comment) about moving big changes to the review process of #127.

The DEBUG_INTERPOLATE flag has been wrapped around all debug messages in #127.

[tfoote]

Sure we can merge this as is now and then target the suggestions from #142 onto #127 instead of this one.