-
Notifications
You must be signed in to change notification settings - Fork 14
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
Conversation
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Checks were failing because Not sure why it was hanging though. It hangs locally for me too. I didn't look closely at what that test is checking. |
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
Signed-off-by: Mabel Zhang <[email protected]>
I have some suggestions on #85 so we avoid hardcoding specific science data |
Signed-off-by: Louise Poubel <[email protected]>
There was a problem hiding this 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
// Lock for changes to worldOriginSpherical* until update complete | ||
std::lock_guard<std::mutex> lock(mtx); | ||
|
||
if (!this->worldSphericalCoordsInitialized) |
There was a problem hiding this comment.
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.
if (!this->worldSphericalCoordsInitialized) | |
if (this->worldSphericalCoordsInitialized) | |
return; |
I think we can lock the mutex after this block?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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?
0); | |
this->worldOriginSphericalCoords.ElevationReference()); |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
TODO added in c287169
There was a problem hiding this comment.
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()); |
There was a problem hiding this comment.
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:
- Convert
worldOriginSphericalCoords
toworldOriginSphericalPos
- Convert
worldOriginSphericalPos
tosphCoord
Are they supposed to be different?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
TODO added in c287169
There was a problem hiding this comment.
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}; |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
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}; |
There was a problem hiding this comment.
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?
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. |
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 clicked the ^ button to collapse it the display goes away but not the side bar. And when I expanded it to full screen it overlapped the 3D visualization tab. |
That's expected. The side bar takes the full vertical height, even if it doesn't have enough plugins to fill it.
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]>
@mabelzhang , thanks for breaking the PR apart. I have an alternative implementation for the coordinate conversion on #98. Let me know what you think. |
* 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]>
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
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 parameterUpdate: #79), you may want to change theigndbg
messages in this PR toignerr
locally in order to see the debug printouts for visualization.You should see printouts for the first 10 points for validation:
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:
WorldCommPlugin
the first spawned vehicle position, in case the user usesempty_environment.sdf
instead ofbuoyant_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 inConfigure()
, 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.)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.