Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Science data: lat/lon to cartesian #70

Closed
wants to merge 21 commits into from
Closed

Conversation

mabelzhang
Copy link
Collaborator

@mabelzhang mabelzhang commented Nov 10, 2021

I was going to break https://github.com/osrf/lrauv_private/pull/133 into 3 parts, but turns out I can’t cherry-pick from the original branch in the private repo, because the branch is transferred to the new repo as a single commit.

Just as well, because without the whole thing, you won’t be able to test the visualization anyway. It needs the optimization flags and hacks, otherwise it’ll either lag or be visualized on a scale that the orbiting tools won’t allow you to see.

So it’s just 1 PR, hopefully not too big.

Many performance tricks are added, which take the shape of constant variables. These are needed to see anything at all even just for sanity checks. See Future Work section below.

To test

ign gazebo buoyant_tethys.sdf -v 4

Unpause.
Expand “Visualize science data” plugin panel on upper-right.
Click the orange refresh button in the panel.
Try the 3 topics in the drop-down list: /chlorophyll, /salinity, /temperature.
You may need to wait a few seconds for the data to go through the pipeline.
Different colors should show up, looking like this video demo I showed at ROS World:

2021-10-20-03-12_scienceDataVis_fullSet_trim21s_compress2000000bitrate_final.mp4

The spam from TethysCommPlugin.cc is very obnoxious (we need a way to easily disable that, perhaps as a SDF plugin parameter Update: #79), you may want to change the igndbg messages in this PR to ignerr locally in order to see the debug printouts for visualization.

You should see printouts for the first 10 points for validation:

[GUI] [Dbg] [VisualizePointCloud.cc:508] Visualizing 404 markers
[GUI] [Dbg] [VisualizePointCloud.cc:383] First point in cloud (size 92355): -1.22956, -16.1997, -0
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 0 at -5.94342, -14.7471, -0, value 16.3002, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 1 at -5.48038, -13.4784, -0, value 16.4099, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 2 at -6.6232, -13.1163, -0, value 17.3562, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 3 at -7.75394, -12.7543, -0, value 17.108, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 4 at -3.86382, -12.5734, -0, value 17.1428, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 5 at -8.8726, -12.3926, -0, value 17.5717, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 6 at -5.01322, -12.2118, -0, value 16.1063, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 7 at -6.15055, -11.8503, -0, value 17.4024, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 8 at -7.27584, -11.489, -0, value 16.9869, type /temperature, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:489] Added point 9 at -3.39804, -11.3084, -0, value 17.14, type /temperature, dimX 0.38088

Future Work

There are still a number of necessary things to do.
Some are currently worked around. Some are hacked around.

World origin lat/long:

  • Visualization needs to poll from WorldCommPlugin the first spawned vehicle position, in case the user uses empty_environment.sdf instead of buoyant_tethys.sdf. The former allows a vehicle to be spawned at any lat/long. The latter has the lat/long hardcoded. Currently, the visualization only works with the latter and will have the wrong Cartesian coordinates for the former. It might be done with a service, but WHEN is the question. It cannot be done in Configure(), because the user might wait 10 minutes to spawn a vehicle. It might need to be done every once in a while.
    (Update: This might be done by Science sensors: detect when spherical coordinates are set #77 and 3f6edfc, but it needs to be tested with empty_environment.sdf and a new vehicle being spawned.)
  • After getting that new lat/long, all the science data positions, which would have already been read and converted to Cartesian, need to be transformed to the new lat/long that the world origin is associated with.
  • The static world lat/long is hardcoded right now. This should be read from the SDF. (Update: Science sensors: detect when spherical coordinates are set #77 and 3f6edfc)

Sensor coordinates:

Visualization:

  • Per-vertex coloring for Markers as tracked in Ignition Marker improvements for LRAUV #52. This should give better performance. Performance is the main bottleneck that requires all the flags tagged Performance trick in the code.

  • Orbiting tool improvements for larger worlds. Currently, the markers are scaled down to abysmally small scales, in order to get them into view, just for our debugging purposes. Otherwise there is no way to even see where the data are for sanity checks.

@mabelzhang
Copy link
Collaborator Author

mabelzhang commented Nov 16, 2021

Checks were failing because test_spawn was hanging.
I disabled science sensors in empty_environment.sdf, since we don't shift all the data to the dynamic new world lat/long yet. So it's not meaningful to load science sensors in that world.

Not sure why it was hanging though. It hangs locally for me too. I didn't look closely at what that test is checking.

@chapulina
Copy link
Contributor

I have some suggestions on #85 so we avoid hardcoding specific science data

Copy link
Contributor

@chapulina chapulina left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Working for me, I just have some minor comments / questions / suggestions

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc Outdated Show resolved Hide resolved
lrauv_ignition_plugins/src/ScienceSensorsSystem.cc Outdated Show resolved Hide resolved
lrauv_ignition_plugins/src/ScienceSensorsSystem.cc Outdated Show resolved Hide resolved
// Lock for changes to worldOriginSpherical* until update complete
std::lock_guard<std::mutex> lock(mtx);

if (!this->worldSphericalCoordsInitialized)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nitpick: In situations like this, it looks a bit cleaner if you return early on the opposite condition. It's easier to read the code if we know that the function ends here, and also reduces indentation of the block below.

Suggested change
if (!this->worldSphericalCoordsInitialized)
if (this->worldSphericalCoordsInitialized)
return;

I think we can lock the mutex after this block?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, we cannot return right away. That's actually a logic error due to incorrect braces. Thanks for catching that.

The if-statement is there because UpdateWorldSphericalOrigin() is called from two places, one in Configure() (because otherwise no data is shown), and the other in PreUpdate(). In the former case, the variable worldSphericalCoordsInitialized is false, and in the latter case, it is true. BUT, in both cases, ShiftDataToNewSphericalOrigin() needs to be called. So the ShiftDataToNewSphericalOrigin() call is supposed to go outside the if-statement.

That hadn't been caught because ShiftDataToNewSphericalOrigin() is not coded yet! So both cases the behavior is the same.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I pulled the function call out of the if-statement (to be committed, will post hash), but the logic might need to be changed once the function is coded. The logic is a combination of this->worldSphericalCoordsInitialized and this->initialized, which needs to be thought about and tested after ShiftDataToNewSphericalOrigin() is implemented.

Really, I just put the calls in there for whoever that implements ShiftDataToNewSphericalOrigin() to be aware of these different circumstances under which UpdateWorldSphericalOrigin() may be called. The call to UpdateWorldSphericalOrigin() in PreUpdate() needs to be tested at the same time too, because that condition has also never happened so far. It only happens if a vehicle is spawned.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TODOs added in c287169

this->worldOriginSphericalPos = ignition::math::Vector3d(
this->worldOriginSphericalCoords.LatitudeReference().Degree(),
this->worldOriginSphericalCoords.LongitudeReference().Degree(),
0);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Any reason to hardcode 0 here?

Suggested change
0);
this->worldOriginSphericalCoords.ElevationReference());

Copy link
Collaborator Author

@mabelzhang mabelzhang Nov 24, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There's something weird about the conversion. I don't know if it's because I'm using the function wrong, or we're not using Z the way the function assumes.

The depth conversion result looks wrong if I give it the actual elevation, in that it doesn't give us -depth as z. Since we're treating z = 0 as surface, and z is supposed to be -depth, I just do the lat/long transformation with elevation 0, and then put in the z manually. That is done in ConvertLatLonToCart() (it's a function because I use it more than once in newer code in #83, some of which I backported to here) as well as here.

Short of a fix, I can add a TODO linking to this comment.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TODO added in c287169

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How can I reproduce the failure? I see buoyant_tethys.sdf is currently setting the <elevation> to zero, so this change here shouldn't make a difference.

0);
// Convert spherical coordinates to Cartesian
this->sphCoord = ignition::math::SphericalCoordinates(
this->worldOriginSphericalCoords.Surface());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's not clear to me what's the intended difference between sphCoord and worldOriginSphericalCoords. What this is doing is:

  1. Convert worldOriginSphericalCoords to worldOriginSphericalPos
  2. Convert worldOriginSphericalPos to sphCoord

Are they supposed to be different?

Copy link
Collaborator Author

@mabelzhang mabelzhang Nov 24, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

They gave me different results. I don't know why, but I had to create a new one for the markers to be at reasonable places. The difference is that worldOriginSphericalCoords contains everything about the world origin, and sphCoord is a blank object that only contains Surface(), which is probably wrong, but it gives results at the origin, which there should be, since the origin is set to be at a lat/long copied straight from the CSV.

I tried it again with this fix in both places in the code, mentioned here #70 (comment) , and the former is still giving positions like this:

[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 0 at -440.367, 365.239, -0, value 0.922332, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 1 at -439.904, 366.508, -0, value 0.437973, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 2 at -441.047, 366.87, -0, value 0.104822, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 3 at -442.178, 367.232, -0, value 0.0355906, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 4 at -438.288, 367.413, -0, value 0.268712, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 5 at -443.296, 367.594, -0, value 0.426041, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 6 at -439.437, 367.775, -0, value 0.527025, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 7 at -440.574, 368.136, -0, value 0.207158, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 8 at -441.7, 368.497, -0, value 0.954847, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 9 at -437.822, 368.678, -0, value 0.0730943, dimX 0.38088

sphCoords gives points near the origin, which I set in the SDF to an exact lat/long that has a data point, so this looks more correct:

[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 0 at 0.989183, -3.98017, -0, value 0.922332, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 1 at 1.45222, -2.71143, -0, value 0.437973, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 2 at 0.309398, -2.34932, -0, value 0.104822, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 3 at -0.821341, -1.98739, -0, value 0.0355906, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 4 at 3.06878, -1.80649, -0, value 0.268712, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 5 at -1.94, -1.62567, -0, value 0.426041, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 6 at 1.91938, -1.44486, -0, value 0.527025, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 7 at 0.782051, -1.08337, -0, value 0.207158, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 8 at -0.343238, -0.722056, -0, value 0.954847, dimX 0.38088
[GUI] [Dbg] [VisualizePointCloud.cc:477] Added point 9 at 3.53456, -0.541467, -0, value 0.0730943, dimX 0.38088

I will say at the time this was coded, I just needed something, anything, to show up at the origin, so a quick hack was needed. So while sphCoord looked wrong to me, I didn't have time to fix it.

Here's what I would propose. Right now, since #83 is pending on this, I would propose to delay the fix again, so that #83 isn't blocked. I know that's not good, but I also suspect after #83 and another PR to do the interpolation, then we can actually test the sensor locations and maybe figure out why this is wrong.

Very honestly, right now, there is still realistically nothing to test this code. We see some colors, but we really have no idea whether they're correct wrt neither lat/long nor Cartesian! So all of these PRs are just code that won't be tested until a few weeks later (but hopefully much much sooner, like before end of month fingers crossed), when we run the MBARI acceptance mission.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TODO added in c287169

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would propose to delay the fix again, so that #83 isn't blocked

I'm ok fixing this incrementally, my only concern is that the positions could be completely wrong right now. Would that affect how #83 is implemented?

right now, there is still realistically nothing to test this code

I think we can come up with some tests, we just need to create some dummy data that we know exactly where it should go. In fact, I think we should be working against this kind of data before we try to load the real datasets, because it's much easier to create different scenarios to test.

My suggestion would be to add more datapoints to dummy.csv and check that they behave as expected. I'll do that now locally here to help me wrap my head around this PR.


/// \brief World origin in spherical position (latitude, longitude,
/// elevation), angles in degrees
public: ignition::math::Vector3d worldOriginSphericalPos = {0, 0, 0};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need to store this variable? I don't see it being used anywhere besides right after it is set.

Copy link
Collaborator Author

@mabelzhang mabelzhang Nov 24, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I print this out in ReadData() in #83. That might be another forward-merge thing. I think this is a useful variable that I'll be using for debugging until all the sensor stuff is finalized. We're still pretty far from that, at least 2 more PRs, before actually testing the missions, which will probably be 1 more PR.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it's just holding duplicate data that's already in worldOriginSphericalCoords, right? Couldn't you just print the values from that? Keeping the same information in 2 places is prone to error because we need to keep them in sync.

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc Outdated Show resolved Hide resolved
lrauv_ignition_plugins/src/ScienceSensorsSystem.cc Outdated Show resolved Hide resolved
This was referenced Nov 20, 2021
@mabelzhang
Copy link
Collaborator Author

Ready for another pass.

I know I'm pushing to delay some fixes, which is not good style, but I'm really anxious to get this in, since #83 depends on this, and I'm working on another one depending on that. I really don't want to chain up more than 3 PRs. We can't run the acceptance test until all 3 PRs mentioned are in, and we only have 3 weeks until we said we wanted to finish the milestone.


/// \brief World origin in Cartesian coordinates converted from spherical
/// coordinates
public: ignition::math::Vector3d worldOriginCartesianCoords = {0, 0, 0};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand this variable. Isn't the world origin in Cartesian always zero? What does a non-zero cartesian origin mean?

@chapulina
Copy link
Contributor

I was going to break osrf/lrauv_private#133 into 3 parts, but turns out I can’t cherry-pick from the original branch

Sorry, that's on me.

I do think it would be beneficial to break this PR into smaller ones though, because there's a lot going on here. It will help with review. I can help divvy-up the work if you want.

@mabelzhang
Copy link
Collaborator Author

Per discussion offline, #93 breaks off the visualization part of this PR, most of which we already looked at in #85.
After that's merged, this PR should only contain the lat/long to Cartesian conversion.

@tfoote
Copy link
Collaborator

tfoote commented Nov 29, 2021

In testing for #83 I was unable to to collapse the widget after I expanded it to the side bar. I don't know if that's related to this or generally an issue

sidebar:
Screenshot from 2021-11-29 12-54-40

clicked the ^ button to collapse it the display goes away but not the side bar.

Screenshot from 2021-11-29 12-54-47

And when I expanded it to full screen it overlapped the 3D visualization tab.

Screenshot from 2021-11-29 12-53-13

@chapulina
Copy link
Contributor

clicked the ^ button to collapse it the display goes away but not the side bar.

That's expected. The side bar takes the full vertical height, even if it doesn't have enough plugins to fill it.

And when I expanded it to full screen it overlapped the 3D visualization tab.

The plugins can either be floating on top of each other, or docked into the side bar. When it's floating like this, you can just drag it somewhere else.

We don't have support for more complex docking arrangements on Ignition GUI, mainly because that was not available out-of-the-box for QtQuick when we started.

Signed-off-by: Louise Poubel <[email protected]>
@chapulina chapulina changed the title Science data visualization (salinity temperature chlorophyll) Science data: lat/lon to cartesian Nov 29, 2021
@chapulina
Copy link
Contributor

@mabelzhang , thanks for breaking the PR apart.

I have an alternative implementation for the coordinate conversion on #98. Let me know what you think.

@mabelzhang
Copy link
Collaborator Author

Closing in favor of #98
Summary: This PR is replaced by #93 and #98

@mabelzhang mabelzhang closed this Dec 3, 2021
@chapulina chapulina deleted the mabelzhang/science_viz branch December 4, 2021 02:19
mabelzhang added a commit that referenced this pull request Dec 17, 2021
* Science data interpolation using raw matrix for depth slice search

* Align lat long in science data visualization with spawned vehicle lat long

* resolve conflicts with main

Signed-off-by: Mabel Zhang <[email protected]>

* replace TODO TEMPORARY HACK comments with Performance trick

Signed-off-by: Mabel Zhang <[email protected]>

* cleanup

Signed-off-by: Mabel Zhang <[email protected]>

* add mutex for worldOriginSpherical*

Signed-off-by: Mabel Zhang <[email protected]>

* finish merge

Signed-off-by: Mabel Zhang <[email protected]>

* better debug msgs and var names

Signed-off-by: Mabel Zhang <[email protected]>

* add BUILD_TESTING to README

Signed-off-by: Mabel Zhang <[email protected]>

* disable science sensors in empty_environment.sdf

Signed-off-by: Mabel Zhang <[email protected]>

* fix merge

Signed-off-by: Mabel Zhang <[email protected]>

* Convert sensor lat long to Cartesian. Some cleanup.

Signed-off-by: Mabel Zhang <[email protected]>

* refactor conversion from lat long to cart into fn

Signed-off-by: Mabel Zhang <[email protected]>

* manual cherry pick 643c97d from mabelzhang/interpolate_sci_raw_mat

Signed-off-by: Mabel Zhang <[email protected]>

* remove WorldOriginSphericalService from WorldCommPlugin

Signed-off-by: Mabel Zhang <[email protected]>

* add depth to ConvertLatLonToCart for convenience

Signed-off-by: Mabel Zhang <[email protected]>

* incorporate updated ConvertLatLonToCart depth

Signed-off-by: Mabel Zhang <[email protected]>

* clean up FindInterpolators() and SearchInDepthSlice()

Signed-off-by: Mabel Zhang <[email protected]>

* Found 4 NNs in each of 2 z slices holding 1st and 2nd NNs

Signed-off-by: Mabel Zhang <[email protected]>

* add headers to organize the gazillion member fields. reorder fns

Signed-off-by: Mabel Zhang <[email protected]>

* decrease how often interpolation is called

Signed-off-by: Mabel Zhang <[email protected]>

* decrease how often interpolation is called, so sci data vis can be shown

Signed-off-by: Mabel Zhang <[email protected]>

* return 2 sets of 4 points from FindInterpolators()

Signed-off-by: Mabel Zhang <[email protected]>

* cleanup

Signed-off-by: Mabel Zhang <[email protected]>

* Suggestions to #70 (#85)

Signed-off-by: Louise Poubel <[email protected]>

* PR comments

Signed-off-by: Mabel Zhang <[email protected]>

* minor PR comments

Signed-off-by: Mabel Zhang <[email protected]>

* refactor to use pcl PassThrough filter

Signed-off-by: Mabel Zhang <[email protected]>

* trilinear interpolation math; API WIP; merge from mabelzhang/interpolate_sci_raw_mat

Signed-off-by: Mabel Zhang <[email protected]>

* fix variable name

Signed-off-by: Mabel Zhang <[email protected]>

* Fix trilinear interpolation. Find ordering in prism for 8 input vertices, match data accordingly; concatenate 2 input arrays into 1; correct inputs to trilinear interpolation

Signed-off-by: Mabel Zhang <[email protected]>

* better printouts for debugging whether points are on prism

Signed-off-by: Mabel Zhang <[email protected]>

* add barycentric interpolation

Signed-off-by: Mabel Zhang <[email protected]>

* remove trilinear interpolation

Signed-off-by: Mabel Zhang <[email protected]>

* remove extra line

Signed-off-by: Mabel Zhang <[email protected]>

* remove more trilinear stuff; printouts

Signed-off-by: Mabel Zhang <[email protected]>

* move comment

Signed-off-by: Mabel Zhang <[email protected]>

* update comment

Signed-off-by: Mabel Zhang <[email protected]>

* add TODOs

Signed-off-by: Mabel Zhang <[email protected]>

* Fix logic error to interpolate data for ALL sensors; add 1D degenerative case for interpolation

Signed-off-by: Mabel Zhang <[email protected]>

* fix zero row check - abs val needed

Signed-off-by: Mabel Zhang <[email protected]>

* Check negative lambda to find correct 2D triangle that query point lies within

Signed-off-by: Mabel Zhang <[email protected]>

* 1D case more accurate, use closest points; fix bug on weights

Signed-off-by: Mabel Zhang <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants