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.

Sign up for GitHub

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

Remove miniature scale from science data and fix segfault #126

Merged
merged 3 commits into from
Dec 23, 2021
+72 −43
Merged

Remove miniature scale from science data and fix segfault #126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1e6697d
Remove miniature scale from science data and fix segfault
chapulina Dec 22, 2021
b93cf31
Merge branch 'main' into chapulina/barycentric_segfault
chapulina Dec 22, 2021
b8641f0
actually store the sorted 1D neighbors
mabelzhang Dec 23, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 40 additions & 27 deletions lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,9 +104,9 @@ class tethys::ScienceSensorsSystemPrivate
const std::vector<int> &_inds,
std::vector<float> &_new);

/// \brief Sort vector in-place, keeping track of indices
/// \brief Sort vector, store indices to original vector after sorting.
/// Original vector unchanged.
/// \param[in] _v Vector to sort
/// \param[out] _v Sorted vector
/// \param[out] _idx Indices of original vector in sorted vector
public: template<typename T>
void SortIndices(
Expand Down Expand Up @@ -296,18 +296,6 @@ class tethys::ScienceSensorsSystemPrivate
/// \brief Publish a few more times for visualization plugin to get them
public: int repeatPubTimes = 1;

//////////////////////////////
// Constants for visualization

// TODO This is a workaround pending upstream Ignition orbit tool improvements
// \brief Scale down in order to see in view
// For 2003080103_mb_l3_las_1x1km.csv
//public: const float MINIATURE_SCALE = 0.01;
// For 2003080103_mb_l3_las.csv
public: const float MINIATURE_SCALE = 0.0001;
// For simple_test.csv
//public: const float MINIATURE_SCALE = 1.0;

// TODO This is a workaround pending upstream Marker performance improvements.
// \brief Performance trick. Skip depths below this z, so have memory to
// visualize higher layers at higher resolution.
Expand Down Expand Up @@ -567,11 +555,6 @@ void ScienceSensorsSystemPrivate::ReadData(
// Flip sign of z, because positive depth is negative z.
cart.Z() = -depth;

// Performance trick. Scale down to see in view
cart *= this->MINIATURE_SCALE;
// Revert Z to the unscaled depth
cart.Z() = -depth;

// Performance trick. Skip points beyond some distance from origin
if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000)
{
Expand Down Expand Up @@ -897,16 +880,22 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
return std::numeric_limits<float>::quiet_NaN();
}

// Sort points
// Sort points and store the indices
std::vector<float> xsSorted;
std::vector<size_t> xsSortedInds;
for (int i = 0; i < _xs.size(); ++i)
{
xsSorted.push_back(_xs(i));
}
SortIndices(xsSorted, xsSortedInds);
// Access sorted indices to get the new sorted array
for (int i = 0; i < xsSortedInds.size(); ++i)
{
xsSorted[i] = _xs(xsSortedInds[i]);
}

int ltPSortedIdx, gtPSortedIdx;
int ltPSortedIdx{-1};
int gtPSortedIdx{-1};
float ltPDist, gtPDist;

// Get the two closest positions in _xs that _p lies between.
Expand All @@ -926,13 +915,42 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
}
}

// Sanity check
if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() ||
gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size())
{
ignwarn << "1D linear interpolation: cannot find pair of consecutive "
<< "neighbors that query point lies between. Cannot interpolate. "
<< "(This should not happen!)"
<< std::endl;
if (this->DEBUG_INTERPOLATE)
{
igndbg << "Neighbors: " << std::endl << _xs << std::endl;
igndbg << "Sorted neighbors: " << std::endl;
for (int i = 0; i < xsSorted.size(); ++i)
igndbg << xsSorted[i] << std::endl;
igndbg << "Query point: " << std::endl << _p << std::endl;
}
return std::numeric_limits<float>::quiet_NaN();
}

// Normalize the distances to ratios between 0 and 1, to use as weights
float ltPWeight = ltPDist / (gtPDist + ltPDist);
float gtPWeight = gtPDist / (gtPDist + ltPDist);

// Retrieve indices of sorted elements in original array
int ltPIdx = xsSortedInds[ltPSortedIdx];
int gtPIdx = xsSortedInds[gtPSortedIdx];

// Sanity check
if (ltPIdx >= _values.size() || gtPIdx >= _values.size())
{
ignwarn << "1D linear interpolation: mapping from sorted index to "
<< "original index resulted in invalid index. Cannot interpolate. "
<< "(This should not happen!)"
<< std::endl;
return std::numeric_limits<float>::quiet_NaN();
}
float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx];

if (this->DEBUG_INTERPOLATE)
Expand Down Expand Up @@ -975,7 +993,7 @@ void ScienceSensorsSystemPrivate::SortIndices(

// Sort indexes based on comparing values in v using std::stable_sort instead
// of std::sort to avoid unnecessary index re-orderings when v contains
// elements of equal values
// elements of equal values
std::stable_sort(_idx.begin(), _idx.end(),
[&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];});
}
Expand Down Expand Up @@ -1396,11 +1414,6 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
{"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32},
});

// TODO optimization for visualization:
// Use PCL methods to chop off points beyond some distance from sensor
// pose. Don't need to visualize beyond that. Might want to put that on a
// different topic specifically for visualization.

msg.mutable_header()->mutable_stamp()->set_sec(this->timestamps[this->timeIdx]);

pcl::PCLPointCloud2 pclPC2;
Expand Down
48 changes: 32 additions & 16 deletions lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,17 @@
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<!-- looking at robot -->
<camera_pose>0 6 6 0 0.5 -1.57</camera_pose>
<!-- looking at all science data for 2003080103_mb_l3_las.csv -->
<!--camera_pose>-50000 -30000 250000 0 1.1 1.58</camera_pose-->
<camera_clip>
<!-- ortho view needs low near clip -->
<!-- but a very low near clip messes orbit's far clip ?! -->
<near>0.1</near>
<!-- See 3000 km away -->
<far>3000000</far>
</camera_clip>
</plugin>

<!-- Plugins that add functionality to the scene -->
Expand Down Expand Up @@ -260,10 +270,25 @@
<property type="string" key="state">docked_collapsed</property>
</ignition-gui>
</plugin>
<plugin filename="ViewAngle" name="Camera controls">
<ignition-gui>
<title>Camera controls</title>
<property type="string" key="state">docked_collapsed</property>
</ignition-gui>
</plugin>
<plugin filename="GridConfig" name="Grid config">
<ignition-gui>
<property type="string" key="state">docked_collapsed</property>
</ignition-gui>
<insert>
<!-- 300 km x 300 km -->
<cell_count>6</cell_count>
<vertical_cell_count>0</vertical_cell_count>
<!-- 50 km -->
<cell_length>50000</cell_length>
<pose>0 100000 0 0 0 0.32</pose>
<color>0 1 0 1</color>
</insert>
<insert>
<!-- 0.1 km x 0.1 km -->
<cell_count>100</cell_count>
Expand Down Expand Up @@ -303,32 +328,23 @@
<direction>-0.5 0.1 -0.9</direction>
</light>

<!-- Uncomment if you need a ground plane -->
<!-- <model name="ground_plane">

<!-- This invisible plane helps with orbiting the camera, especially at large scales -->
<model name="horizontal_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
<!-- 300 km x 300 km -->
<size>300000 300000</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
<transparency>1.0</transparency>
</visual>
</link>
</model> -->
</model>

<!-- Uncomment for particle effect
Requires ParticleEmitter2 in ign-gazebo 4.8.0, which will be copied
Expand Down