Trilinear and bary-linear interpolations

[mabelzhang]

Some Christmas lights before the holidays.

Builds on top of #83

This PR adds trilinear interpolation and a "bary-linear" interpolation when points don't satisfy the requirements of trilinear interpolation.

• TrilinearInterpolate() first does a long series of checks to make sure the 8 points are indeed vertices of a prism. Then, it implements the trilinear interoplation on Wikipedia
• BaryLinearInterpolate() is a custom hybrid approach combining barycentric and linear interpolation that I hope works well. It reuses the functions written for barycenric and trilinear interpolations.
  It is called when TrilinearInterpolate() detects the 8 points are not vertices of a prism, which happens when the robot is on the boundaries.
  In this case, the 4 points on each z slice are treated as a degenerated tetrahedra, and the 2D overloaded version of BarycentricInterpolate() is called. The result is (x, y, z1), (x, y, z2), where (x, y, z) is the query point, and z1 and z2 are positions of the two z slices.
  Then, these two intermediate points are linearly interpolated (along z) to get the data value at (x, y, z).

With the latter, I can finally test simple data reasonably without running into "the algorithm doesn't even finish because the data doesn't satisfy the requirements."

Bookkeeping of the indices of the sub-cloud and sub-sub-cloud and mapping back to the original cloud is a bit crazy tedious. I think I shook out all the bugs.

Test case 1 (easy)

"Normal" use case where the robot's 8 interpolation neighbors are on the vertices of a prism, i.e. satisfies trilinear interpolation requirement.
Robot is exactly on a z slice.

interpolation_prism.csv:

Robot at

      <pose>4 -5 0 0 0 0</pose>

Turn on DEBUG_INTERPOLATE = true to see the result printouts.
Interpolation result is 300, value of all the points on the top z slice robot is on:

[Dbg] [ScienceSensorsSystem.cc:2019] Interpolating chlorophyll
[Dbg] [ScienceSensorsSystem.cc:1070] Trilinear interpolation min vert v000: 3.33958, -6.63446, -2
[Dbg] [ScienceSensorsSystem.cc:1072] Trilinear interpolation max vert v111: 6.67917, -3.31723, -0
[Dbg] [ScienceSensorsSystem.cc:1169] Trilinear interpolation, starting with 8 points: 100, 300, 100, 300, 100, 300, 100, 300
[Dbg] [ScienceSensorsSystem.cc:1192] Trilinear interpolation, 4 intermediate results from 8 points: 100, 300, 100, 300
[Dbg] [ScienceSensorsSystem.cc:1201] Trilinear interpolation, 2 intermediate results from 4 points: 100, 300
[Dbg] [ScienceSensorsSystem.cc:1210] Trilinear interpolation result: 300

Test case 2

More data points.
Robot is in a prism and between two z slices.

interpolation_test.csv:

Robot at

      <pose>4 -5 -1 0 0 0</pose>

Interpolation result is 200, midpoint of 100 and 300:

[Dbg] [ScienceSensorsSystem.cc:1165] Trilinear interpolation, starting with 8 points: 100, 300, 100, 300, 100, 300, 100, 300
[Dbg] [ScienceSensorsSystem.cc:1188] Trilinear interpolation, 4 intermediate results from 8 points: 100, 300, 100, 300
[Dbg] [ScienceSensorsSystem.cc:1197] Trilinear interpolation, 2 intermediate results from 4 points: 100, 300
[Dbg] [ScienceSensorsSystem.cc:1206] Trilinear interpolation result: 200

Test case 3 (hard)

interpolation_test.csv still.
Robot is on the boundary, its nearest neighbors not a prism

      <pose>0 0 0 0 0 0</pose>

Hybrid barylinaer interpolation is activated.
Interpolation result is 250:

[Dbg] [ScienceSensorsSystem.cc:1010] Found 4 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, 1.106, -0), distance 1.10574 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -1.106, -0), distance 1.10574 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, 2.211, -0), distance 2.21149 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -3.317, -0), distance 3.31723 m
[Dbg] [ScienceSensorsSystem.cc:873] Excluding 1st nn z slice. Remaining cloud has 15 points
[Dbg] [ScienceSensorsSystem.cc:1010] Found 1 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (3.34, 0, -2), distance 3.89266 m
[Dbg] [ScienceSensorsSystem.cc:911] 2nd nn (-3.33958, 0, -2), idx 8, dist 3.89266, z slice 15 points
[Dbg] [ScienceSensorsSystem.cc:1010] Found 4 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (3.34, 0, -2), distance 3.89266 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (-3.34, 0, -2), distance 3.89266 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (3.34, -3.317, -2), distance 5.11437 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (3.34, 3.317, -2), distance 5.11437 m
[Dbg] [ScienceSensorsSystem.cc:1913] interpolatorInds1.size(): 4
…
[Dbg] [ScienceSensorsSystem.cc:2015] Interpolating chlorophyll
[Dbg] [ScienceSensorsSystem.cc:1066] Trilinear interpolation min vert v000: -3.33958, -3.31723, -2
[Dbg] [ScienceSensorsSystem.cc:1068] Trilinear interpolation max vert v111: 3.33958, 3.31723, -0
[Wrn] [ScienceSensorsSystem.cc:1152] Trilinear interpolation: Suspect 8 input points not on prism. Vertex 0 (0, 1.106, -0) not within tolerance (1e-06) of any of 8 vertices of rectangular prism. Using hybrid bary-linear interpolation instead.
…
[Dbg] [ScienceSensorsSystem.cc:1399] Barycentric 2D lambda 1 2 3: 0.5, 0.5, 0
[Dbg] [ScienceSensorsSystem.cc:1418] Barycentric 2D interpolation of values 300, 200, 200 resulted in 250
[Dbg] [ScienceSensorsSystem.cc:1618] Hybrid bary-linear interpolation, 2 barycentric interpolations on 2 z slices resulted in: 250 and 250
[Dbg] [ScienceSensorsSystem.cc:1637] Hybrid bary-linear interpolation, linear interpolation of 2 points on two z slices: 250
[Dbg] [ScienceSensorsSystem.cc:1643] Hybrid bary-linear interpolation of 8 values:
[Dbg] [ScienceSensorsSystem.cc:1646] 300
[Dbg] [ScienceSensorsSystem.cc:1646] 200
[Dbg] [ScienceSensorsSystem.cc:1646] 200
[Dbg] [ScienceSensorsSystem.cc:1646] 200
[Dbg] [ScienceSensorsSystem.cc:1646] 100
[Dbg] [ScienceSensorsSystem.cc:1646] 300
[Dbg] [ScienceSensorsSystem.cc:1646] 100
[Dbg] [ScienceSensorsSystem.cc:1646] 100
[Dbg] [ScienceSensorsSystem.cc:1647] resulted in 250

Test case 4 (real data)

Real data

      <data_path>2003080103_mb_l3_las.csv</data_path>

Because real data is ugly, it's hard to tell whether things work by reading numbers.
The standard in this test case is: It doesn't crash, and the interpolation result doesn't look bonkers from the data it's interpolating from.
(For functionality, use test cases 1-3 and the simple test files.)

Robot on boundary (not in prism). Activates hybrid bary-linear.

[Dbg] [ScienceSensorsSystem.cc:692] At time slice 0, populated 295536 spatial coordinates.
[Dbg] [ScienceSensorsSystem.cc:1858] Searching around sensor Cartesian location 0, -0, 0
[Dbg] [ScienceSensorsSystem.cc:1893] Neighbor at (0, -0, -0), squared distance 1.94831e-16 m
[Dbg] [ScienceSensorsSystem.cc:818] 295536 points in full cloud
[Dbg] [ScienceSensorsSystem.cc:826] 1st nn (1.39148e-08, -1.1001e-09, -0), idx 4021, dist 1.39582e-08, z slice 18471 points
[Dbg] [ScienceSensorsSystem.cc:1010] Found 4 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -0, -0), distance 1.39582e-08 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (-1812.03, 0.184, -0), distance 1812.03 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (1812.03, 0.184, -0), distance 1812.03 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -2218.66, -0), distance 2218.66 m
[Dbg] [ScienceSensorsSystem.cc:873] Excluding 1st nn z slice. Remaining cloud has 277065 points
[Dbg] [ScienceSensorsSystem.cc:1010] Found 1 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -0, -5), distance 5 m
[Dbg] [ScienceSensorsSystem.cc:911] 2nd nn (1.39148e-08, -1.1001e-09, -0), idx 4021, dist 5, z slice 18471 points
[Dbg] [ScienceSensorsSystem.cc:1010] Found 4 neighbors.
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -0, -5), distance 5 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (-1812.03, 0.184, -5), distance 1812.04 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (1812.03, 0.184, -5), distance 1812.04 m
[Dbg] [ScienceSensorsSystem.cc:1018] Neighbor at (0, -2218.66, -5), distance 2218.66 m
[Dbg] [ScienceSensorsSystem.cc:1913] interpolatorInds1.size(): 4
...
[Dbg] [ScienceSensorsSystem.cc:1988] Interpolating salinity
[Dbg] [ScienceSensorsSystem.cc:1066] Trilinear interpolation min vert v000: -1812.03, -2218.66, -5
[Dbg] [ScienceSensorsSystem.cc:1068] Trilinear interpolation max vert v111: 1812.03, 0.184071, -0
[Wrn] [ScienceSensorsSystem.cc:1152] Trilinear interpolation: Suspect 8 input points not on prism. Vertex 0 (0, -0, -0) not within tolerance (1e-06) of any of 8 vertices of rectangular prism. Using hybrid bary-linear interpolation instead.
...
[Dbg] [ScienceSensorsSystem.cc:1399] Barycentric 2D lambda 1 2 3: 8.80114e-16, 6.75847e-27, 1
[Dbg] [ScienceSensorsSystem.cc:1418] Barycentric 2D interpolation of values 33.3166, 33.3209, 33.3119 resulted in 33.3119
[Dbg] [ScienceSensorsSystem.cc:1618] Hybrid bary-linear interpolation, 2 barycentric interpolations on 2 z slices resulted in: 33.3119 and 33.3119
[Dbg] [ScienceSensorsSystem.cc:1637] Hybrid bary-linear interpolation, linear interpolation of 2 points on two z slices: 33.3119
[Dbg] [ScienceSensorsSystem.cc:1643] Hybrid bary-linear interpolation of 8 values:
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3166
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3209
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3119
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3125
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3042
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3068
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3028
[Dbg] [ScienceSensorsSystem.cc:1646] 33.3022
[Dbg] [ScienceSensorsSystem.cc:1647] resulted in 33.3119

Future work

It would really help visual debugging to publish Markers marking where the found neighbors are, and a marker at the robot colored by the interpolation result. Currently we have to look at the numbers and think about it in our head, which doesn't scale when the numbers aren't nice.

[mabelzhang]

To make it simpler. we can get two sides of the 4 per plane, use the dot product with that vector to compute the linear interpolation of each line. Then similarly do the dot product to the linear interpolation between the resulting points to compute the appropriate point in each plane. And with two of those done, it's just linear interpolation vertically with the z values.

I believe this is very similar to what the hybrid bary-linear algorithm in this PR does, in BaryLinearInterpolate(). It is called when the prism constraints for the trilinear interpolation are not met.

This way, we offer 2 alternative interpolation algorithms, so that when the data is in a rectangular grid, they get a result from a stricter interpolation method. I haven't compared empirically whether it is better, but right now we have two alternatives in the code that can be tested when we have more data. It is easy to bypass one of them later if we find that it's unnecessary to have two. It's much harder to add the trilinear back, given how much time it's already taken to implement it.

[tfoote]

Sorry I accidentially pushed the CreateDepthSlice commit to this branch instead of my other one where I'm rebasing #142 https://github.com/osrf/lrauv/compare/tfoote/interpolation_review_rebase_127?expand=1

[mabelzhang]

Succeeded by #190, #191