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

A suggestion for a simpler search algorithm for the interpolation #142

Closed
wants to merge 6 commits into from
Closed

A suggestion for a simpler search algorithm for the interpolation #142

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
619e43e
remove magic number from CreateDepthSlice
tfoote Jan 11, 2022
758eb62
Simplify search logic to take slices first then do search within the …
tfoote Jan 11, 2022
c9130c5
remove unused parameters
tfoote Jan 11, 2022
ed7c60d
add missing pipe
tfoote Jan 11, 2022
79c2aef
bump to 100m to try to catch more in default dataset
tfoote Jan 11, 2022
ce030d4
data is 5m vertically
tfoote Jan 11, 2022
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
105 changes: 23 additions & 82 deletions lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,28 +89,26 @@ class tethys::ScienceSensorsSystemPrivate
/// \brief Find XYZ locations of points in the two closest z slices to
/// interpolate among.
/// \param[in] _pt Location in space to interpolate for
/// \param[in] _inds Indices of nearest neighbors to _pt
/// \param[in] _sqrDists Distances of nearest neighbors to _pt
/// \param[out] _interpolators1 XYZ points on a z slice to interpolate among
/// \param[out] _interpolators2 XYZ points on a second z slice to interpolate
/// among
public: void FindTrilinearInterpolators(
pcl::PointXYZ &_pt,
std::vector<int> &_inds,
std::vector<float> &_sqrDists,
std::vector<pcl::PointXYZ> &_interpolators1,
std::vector<pcl::PointXYZ> &_interpolators2);

/// \brief Create a z slice from a point cloud. Slice contains points sharing
/// the same given z value.
/// \param[in] _depth Target z value
/// \param[in] _min_depth z value
/// \param[in] _max_depth z value
/// \param[in] _cloud Cloud from which to create the z slice
/// \param[out] _zSlice Points in the new point cloud slice at _depth
/// \param[out] _zSliceInds Indices of points in _zSlices in the original
/// point cloud _points
/// \param[in] _invert Invert the filter. Keep everything except the z slice.
public: void CreateDepthSlice(
float _depth,
float _min_depth,
float _max_depth,
pcl::PointCloud<pcl::PointXYZ> &_cloud,
pcl::PointCloud<pcl::PointXYZ> &_zSlice,
std::vector<int> &_zSliceInds,
Expand Down Expand Up @@ -723,11 +721,15 @@ bool ScienceSensorsSystemPrivate::comparePclPoints(
/////////////////////////////////////////////////
void ScienceSensorsSystemPrivate::FindTrilinearInterpolators(
pcl::PointXYZ &_pt,
std::vector<int> &_inds,
std::vector<float> &_sqrDists,
std::vector<pcl::PointXYZ> &_interpolators1,
std::vector<pcl::PointXYZ> &_interpolators2)
{
// TODO(tfoote) magic numbers
// Should be passed in and paramaterized based on the expected data
// distribution height or calculated from the grandularity of the dataset.
float slice_depth = 5; // 5 meter.
float epsilon = 1e-6; // Enough for not equal

// Sanity checks
// Vector not populated
if (this->timeSpaceCoords.size() == 0)
Expand All @@ -739,20 +741,7 @@ void ScienceSensorsSystemPrivate::FindTrilinearInterpolators(
{
return;
}
if (_inds.size() == 0 || _sqrDists.size() == 0)
{
ignwarn << "FindInterpolators(): Invalid neighbors aray size ("
<< _inds.size() << " and " << _sqrDists.size()
<< "). No neighbors to use for interpolation. Returning NaN."
<< std::endl;
return;
}
if (_inds.size() != _sqrDists.size())
{
ignwarn << "FindInterpolators(): Number of neighbors != number of "
<< "distances. Invalid input. Returning NaN." << std::endl;
return;
}


// Two slices of different depth z values
pcl::PointCloud<pcl::PointXYZ> zSlice1, zSlice2;
Expand All @@ -764,87 +753,40 @@ void ScienceSensorsSystemPrivate::FindTrilinearInterpolators(
std::vector<int> interpolatorInds1, interpolatorInds2;
std::vector<float> interpolatorSqrDists1, interpolatorSqrDists2;

// Step 1: 1st NN, in its z slice, search for 4 NNs

// 1st nearest neighbor
// The distances from kNN search are sorted, so can always just take [0]th
int nnIdx = _inds[0];
float minDist = _sqrDists[0];
// Get z of neighbor
float nnZ = this->timeSpaceCoords[this->timeIdx]->at(nnIdx).z;

// Debug output
igndbg << this->timeSpaceCoords[this->timeIdx]->size()
<< " points in full cloud" << std::endl;

// Search in z slice for 4 nearest neighbors in this slice
this->CreateDepthSlice(nnZ, *(this->timeSpaceCoords[this->timeIdx]), zSlice1,
// Search in z slice for 4 nearest neighbors above or equal to the point
this->CreateDepthSlice(_pt.z, _pt.z + slice_depth, *(this->timeSpaceCoords[this->timeIdx]), zSlice1,
zSliceInds1);
igndbg << "1st nn idx " << nnIdx << ", dist " << sqrt(minDist)
<< ", z slice " << zSlice1.points.size() << " points" << std::endl;
igndbg << "1st z slice " << zSlice1.points.size() << " points" << std::endl;
this->CreateAndSearchOctree(_pt, zSlice1,
interpolatorInds1, interpolatorSqrDists1, _interpolators1);
if (interpolatorInds1.size() < 4 || interpolatorSqrDists1.size() < 4)
{
ignwarn << "Could not find enough neighbors in 1st slice z = " << nnZ
ignwarn << "Could not find enough neighbors in 1st slice z = " << _pt.z << " or " << slice_depth << " above"
<< " for trilinear interpolation." << std::endl;
return;
}

// Step 2: exclude z slice of 1st NN from further searches.

// Remove all points in the z slice of the 1st NN, so that the 2nd NN will be
// found in another z slice.
// Set invert flag to get all but the depth slice.
pcl::PointCloud<pcl::PointXYZ> cloudExceptZSlice1;
std::vector<int> indsExceptZSlice1;
this->CreateDepthSlice(nnZ, *(this->timeSpaceCoords[this->timeIdx]),
cloudExceptZSlice1, indsExceptZSlice1, true);
igndbg << "Excluding 1st nn z slice. Remaining cloud has "
<< cloudExceptZSlice1.points.size() << " points" << std::endl;

// Step 3: Look for 2nd NN everywhere except z slice of 1st NN.
// In this 2nd z-slice, search for 4 NNs

// Search for 2nd NN
std::vector<int> inds2;
std::vector<float> sqrDists2;
std::vector<pcl::PointXYZ> nbrs2;
this->CreateAndSearchOctree(_pt, cloudExceptZSlice1,
inds2, sqrDists2, nbrs2, 1);
if (inds2.size() < 1 || sqrDists2.size() < 1)
{
ignwarn << "Could not find 2nd NN among "
<< cloudExceptZSlice1.points.size()
<< " points for trilinear interpolation" << std::endl;
return;
}

// Take closest point as 2nd NN
int nnIdx2 = inds2[0];
float minDist2 = sqrDists2[0];
// Get z of neighbor
float nnZ2 = nbrs2[0].z;

// Step 4: Look for 4 NNs in the z slice of 2nd NN

// Search in z slice of 1st NN for 4 nearest neighbors in this slice
this->CreateDepthSlice(nnZ2, cloudExceptZSlice1, zSlice2, zSliceInds2);
igndbg << "2nd nn idx " << nnIdx2 << ", dist " << sqrt(minDist2)
<< ", z slice " << zSlice2.points.size() << " points" << std::endl;
// Search in z slice below point for 4 nearest neighbors
this->CreateDepthSlice(_pt.z - slice_depth, _pt.z - epsilon, *(this->timeSpaceCoords[this->timeIdx]), zSlice2, zSliceInds2);
igndbg << "2nd z slice " << zSlice2.points.size() << " points" << std::endl;
this->CreateAndSearchOctree(_pt, zSlice2,
interpolatorInds2, interpolatorSqrDists2, _interpolators2);
if (interpolatorInds2.size() < 4 || interpolatorSqrDists2.size() < 4)
{
ignwarn << "Could not find enough neighbors in 2nd slice z = " << nnZ2
ignwarn << "Could not find enough neighbors in 2nd slice z = " << _pt.z << " or " << slice_depth << " below"
<< " for trilinear interpolation." << std::endl;
return;
}
}

/////////////////////////////////////////////////
void ScienceSensorsSystemPrivate::CreateDepthSlice(
float _depth,
float _min_depth,
float _max_depth,
pcl::PointCloud<pcl::PointXYZ> &_cloud,
pcl::PointCloud<pcl::PointXYZ> &_zSlice,
std::vector<int> &_zSliceInds,
Expand All @@ -857,7 +799,7 @@ void ScienceSensorsSystemPrivate::CreateDepthSlice(
passThruFilter.setInputCloud(_cloud.makeShared());
passThruFilter.setNegative(_invert);
passThruFilter.setFilterFieldName("z");
passThruFilter.setFilterLimits(_depth - 1e-6, _depth + 1e-6);
passThruFilter.setFilterLimits(_min_depth, _max_depth);
passThruFilter.filter(_zSlice);

// Get indices of points kept. Default method returns removed indices, so
Expand Down Expand Up @@ -1574,8 +1516,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
// Find 2 sets of 4 nearest neighbors, each set on a different z slice,
// to use as inputs for trilinear interpolation
std::vector<pcl::PointXYZ> interpolatorsSlice1, interpolatorsSlice2;
this->dataPtr->FindTrilinearInterpolators(searchPoint, spatialIdx,
spatialSqrDist, interpolatorsSlice1, interpolatorsSlice2);
this->dataPtr->FindTrilinearInterpolators(searchPoint, interpolatorsSlice1, interpolatorsSlice2);
if (interpolatorsSlice1.size() < 4 || interpolatorsSlice2.size() < 4)
{
ignwarn << "Could not find trilinear interpolators near sensor location "
Expand Down