Barycentric interpolation of science sensor data

[mabelzhang]

Supersedes #83

Long story short, I finished implementing trilinear interpolation (in another branch, mabelzhang/trilinear_interp), which took a lot of sanity checks to make sure the inputs are 8 vertices of a prism, a lot of sorting the data to make sure which corner lies where and use the correct data corresponding to its corner, blah blah... in the end, the inputs didn't even meet the assumptions.

So, I've switched to barycentric interpolation instead. It took a fraction of the time of trilinear interpolation to implement, as in hours as opposed to days and weeks. Most importantly, it works.
Let's just use barycentric, at least for now.

Why barycentric is better than trilinear:

• No assumption that the 8 input points must be the vertices of a rectangular prism.
  This is a big deal.
  The 8 points being structured takes 4 kNN searches to get 2 z-slices from the unstructured point cloud. Searching for the 2nd z-slice requires removing the 1st z-slice, which messes up the indices, so you need to manually keep track in order to retrieve the correct values later for interpolation.
  The prism assumption takes a lot of tedious code to verify inputs, and to match the data to the right corners.
• Barycentric works for any arbitrary unstructured point cloud. Will work for data not on regular grid - generalizable for upstreaming. Barycentric is actually what the trilinear interpolation Wikipedia page says to use, if your data doesn't satisfy the assumptions for trilinear.
• Simpler code, probably 3-5x fewer lines than trilinear

I didn't throw away the trilinear interpolation code in the other branch, in case we want to return to it. If we don't, some weeks later it'll be trashed.

Ignore the many commits (artifacts from adding barycentric on top of trilinear, which is a branch that's been around for a while and I kept merging everything back into). We'll squash and merge anyway.

Sample output

2D case:

The default robot position of 0 0 0 enacts the 2D case, a special case when matrix T has 3rd row all zeros.
Then, the overloaded 2D function taking Eigen::Vector2f is called.

$ ign gazebo -v 4 buoyant_tethys.sdf
...
[Dbg] [ScienceSensorsSystem.cc:625] At time slice 0, populated 92355 spatial coordinates.
[Dbg] [ScienceSensorsSystem.cc:1321] Searching around sensor Cartesian location 0, -0, -0
[Dbg] [ScienceSensorsSystem.cc:1397] Neighbor at (0, -0, -0), squared distance 1.0366e-10 m
[Dbg] [ScienceSensorsSystem.cc:1397] Neighbor at (-0.181, 0, -0), squared distance 0.0328346 m
[Dbg] [ScienceSensorsSystem.cc:1397] Neighbor at (0.181, 0, -0), squared distance 0.0328346 m
[Dbg] [ScienceSensorsSystem.cc:1397] Neighbor at (0, -0.222, -0), squared distance 0.0492243 m
[Dbg] [ScienceSensorsSystem.cc:986] Interpolating E and N currents
[Dbg] [ScienceSensorsSystem.cc:1041] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:1056] T: 
1.51369e-14   -0.181203    0.181203
   0.221866    0.221884    0.221884
          0           0           0
[Dbg] [ScienceSensorsSystem.cc:1062] 4 points on same z slice. Using 2D barycentric interpolation.
[Dbg] [ScienceSensorsSystem.cc:1115] T: 
   -0.181203    -0.362406
-1.84071e-05            0
[Dbg] [ScienceSensorsSystem.cc:1120] r3: 
   0.181203
1.84071e-05
[Dbg] [ScienceSensorsSystem.cc:1125] T.inverse(): 
      -0 -54326.9
-2.75933  27163.5
[Dbg] [ScienceSensorsSystem.cc:1130] Barycentric 2D lambda 1 2 3: 1, 0, 0
[Dbg] [ScienceSensorsSystem.cc:1140] Barycentric 2D interpolation of values -0.0461921, -0.0453666, -0.0431448 resulted in -0.0461921
[Dbg] [ScienceSensorsSystem.cc:1041] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:1056] T: 
1.51369e-14   -0.181203    0.181203
   0.221866    0.221884    0.221884
          0           0           0
[Dbg] [ScienceSensorsSystem.cc:1062] 4 points on same z slice. Using 2D barycentric interpolation.
[Dbg] [ScienceSensorsSystem.cc:1115] T: 
   -0.181203    -0.362406
-1.84071e-05            0
[Dbg] [ScienceSensorsSystem.cc:1120] r3: 
   0.181203
1.84071e-05
[Dbg] [ScienceSensorsSystem.cc:1125] T.inverse(): 
      -0 -54326.9
-2.75933  27163.5
[Dbg] [ScienceSensorsSystem.cc:1130] Barycentric 2D lambda 1 2 3: 1, 0, 0
[Dbg] [ScienceSensorsSystem.cc:1140] Barycentric 2D interpolation of values -0.0472604, -0.0460758, -0.0485462 resulted in -0.0472604

Check that the last line is reasonable - result is same as first vertex, because first vertex is at 0 0 0.

3D case:

Change robot pose in buoyant_tethys.sdf, so that the z is between two z slices:

    <include>
      <pose>0 0 -2.5 0 0 0</pose>
      <uri>tethys_equipped</uri>
    </include>

$ ign gazebo -v 4 buoyant_tethys.sdf
...
[Dbg] [ScienceSensorsSystem.cc:565] At time slice 0, populated 92355 spatial coordinates.
[Dbg] [ScienceSensorsSystem.cc:885] Searching around sensor Cartesian location 0, -0, -2.5
[Dbg] [ScienceSensorsSystem.cc:921] Neighbor at (0, -0, -5), squared distance 6.24995 m
[Dbg] [ScienceSensorsSystem.cc:921] Neighbor at (0, -0, -0), squared distance 6.25005 m
[Dbg] [ScienceSensorsSystem.cc:921] Neighbor at (-0.181, 0, -5), squared distance 6.28278 m
[Dbg] [ScienceSensorsSystem.cc:921] Neighbor at (0.181, 0, -5), squared distance 6.28278 m
[Dbg] [ScienceSensorsSystem.cc:986] Interpolating E and N currents
[Dbg] [ScienceSensorsSystem.cc:606] _p: 
 8.83035e-39
-2.65023e-13
    -2.50001
[Dbg] [ScienceSensorsSystem.cc:621] T: 
   -0.181203    -0.181203    -0.362406
-1.84071e-05 -1.84071e-05            0
           0            5            0
[Dbg] [ScienceSensorsSystem.cc:637] r4: 
   0.181203
1.84071e-05
         -5
[Dbg] [ScienceSensorsSystem.cc:642] T.inverse(): 
      -0 -54326.9     -0.2
       0        0      0.2
-2.75933  27163.5        0
[Dbg] [ScienceSensorsSystem.cc:647] Barycentric 3D lambda 1 2 3 4: 0.500002, 0.499998, 0, 2.98023e-08
[Dbg] [ScienceSensorsSystem.cc:659] Barycentric 3D interpolation of values -0.0936973, -0.0461921, -0.095667, -0.0894169 resulted in -0.0699448
[Dbg] [ScienceSensorsSystem.cc:606] _p: 
 8.83035e-39
-2.65023e-13
    -2.50001
[Dbg] [ScienceSensorsSystem.cc:621] T: 
   -0.181203    -0.181203    -0.362406
-1.84071e-05 -1.84071e-05            0
           0            5            0
[Dbg] [ScienceSensorsSystem.cc:637] r4: 
   0.181203
1.84071e-05
         -5
[Dbg] [ScienceSensorsSystem.cc:642] T.inverse(): 
      -0 -54326.9     -0.2
       0        0      0.2
-2.75933  27163.5        0
[Dbg] [ScienceSensorsSystem.cc:647] Barycentric 3D lambda 1 2 3 4: 0.500002, 0.499998, 0, 2.98023e-08
[Dbg] [ScienceSensorsSystem.cc:659] Barycentric 3D interpolation of values -0.0237542, -0.0472604, -0.0240482, -0.023139 resulted in -0.0355073

Check that the last line is reasonable, somewhere between the vertices.

[tfoote]

I like the simpler approach here. I think that we can improve the logic on the K nearest neighbors by potentially researching/sampling if we notice that we get a lopsided sample. Aka they're all on one side, reslice the data and search for the nearest neighbor on the other side. Such that we guarantee that the tetrahedron surrounds the point. Otherwise the math is invalid. And if we're doing that logic we could probably avoid the 2d reduced case.

[mabelzhang]

0d0b14d makes the math more rigorous, in checking the matrix T for zero rows and handling degenerative cases accordingly (see sample output below).
(Update: done in cded0f1) Added a TODO for picking the correct triangle in the 2D degenerative case. Currently it's just picking an arbitrary triangle, but it should pick the three points in the tetrahedron in which the query point lies. It might lie outside of an arbitrary set.

0d0b14d also fixes a logic error in the for-loop over sensors. There had to be two separate loops, the first one is for positioning, for which only one sensor needs to be checked. The second one is for interpolating the data, which all sensors need to go through, because the input is not just the position but each sensor's individual data.

This is an example 1D case that is now handled, previously it gave NaNs because T has 2 rows of zeros.
This also shows that all 5 types of sensor data are interpolated. Previously, only the E and N current data were.

[Dbg] [ScienceSensorsSystem.cc:593] At time slice 0, populated 5 spatial coordinates.
[Dbg] [ScienceSensorsSystem.cc:1035] Searching around sensor Cartesian location 0, -0, -0
[Dbg] [ScienceSensorsSystem.cc:1067] Neighbor at (0, -0, -0), squared distance 1.0366e-10 m
[Dbg] [ScienceSensorsSystem.cc:1067] Neighbor at (0, -0, -5), squared distance 24.9999 m
[Dbg] [ScienceSensorsSystem.cc:1067] Neighbor at (0, -0, -10), squared distance 99.9998 m
[Dbg] [ScienceSensorsSystem.cc:1067] Neighbor at (0, -0, -15), squared distance 225 m
[Dbg] [ScienceSensorsSystem.cc:1148] Interpolating E and N currents
[Dbg] [ScienceSensorsSystem.cc:638] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:653] T: 
 0  0  0
 0  0  0
15 10  5
[Dbg] [ScienceSensorsSystem.cc:694] 4 points are on a line. Using 1D interpolation.
[Dbg] [ScienceSensorsSystem.cc:825] 1D linear interpolation of values -0.0461921, -0.0936973, -0.0750533, -0.061266 resulted in -0.061266
[Dbg] [ScienceSensorsSystem.cc:638] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:653] T: 
 0  0  0
 0  0  0
15 10  5
[Dbg] [ScienceSensorsSystem.cc:694] 4 points are on a line. Using 1D interpolation.
[Dbg] [ScienceSensorsSystem.cc:825] 1D linear interpolation of values -0.0472604, -0.0237542, 0.0282764, 0.033508 resulted in 0.033508
[Dbg] [ScienceSensorsSystem.cc:1137] Interpolating chlorophyll
[Dbg] [ScienceSensorsSystem.cc:638] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:653] T: 
 0  0  0
 0  0  0
15 10  5
[Dbg] [ScienceSensorsSystem.cc:694] 4 points are on a line. Using 1D interpolation.
[Dbg] [ScienceSensorsSystem.cc:825] 1D linear interpolation of values 0.469759, 0.535851, 0.144706, 0.0848868 resulted in 0.0848872
[Dbg] [ScienceSensorsSystem.cc:1123] Interpolating temperature
[Dbg] [ScienceSensorsSystem.cc:638] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:653] T: 
 0  0  0
 0  0  0
15 10  5
[Dbg] [ScienceSensorsSystem.cc:694] 4 points are on a line. Using 1D interpolation.
[Dbg] [ScienceSensorsSystem.cc:825] 1D linear interpolation of values 17.0189, 16.106, 13.6954, 12.7745 resulted in 12.7746
[Dbg] [ScienceSensorsSystem.cc:1112] Interpolating salinity
[Dbg] [ScienceSensorsSystem.cc:638] _p: 
 8.83035e-39
-2.65023e-13
-1.01813e-05
[Dbg] [ScienceSensorsSystem.cc:653] T: 
 0  0  0
 0  0  0
15 10  5
[Dbg] [ScienceSensorsSystem.cc:694] 4 points are on a line. Using 1D interpolation.
[Dbg] [ScienceSensorsSystem.cc:825] 1D linear interpolation of values 33.3166, 33.3042, 33.2683, 33.2599 resulted in 33.2599

[tfoote]

I've read through the code and seen it running. I'd really like to have unit tests for the interpolation in isolation so that we can be confident in it outside the context of this plugin. But after looking into that there's no good way for us to instrument into the private implementation class.

I'd suggest that we plan in the future to refactor that out in to a standalone library which can be unit tested. But for now we merge this so we can build on top of it. It will be used every run and there's not many branch points so it should be obvious if things aren't working well. And we can defer filling in coverage testing.

[mabelzhang]

Thank you for the review!

Yeah, I hadn't thought about the class being private. As an intermediate to having a standalone library, we might be able to still add tests like with other plugins by checking the sensor output, which should be the interpolation result given the robot location and some known data. I think that should be doable in the test infrastructure.

[mabelzhang]

Only test failing is LrauvTestFixture.PitchMass and our tests are currently known to be flaky.

Will merge this for now and add improvements from manual tests, and add automated tests in new PRs.