diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 3cab97f4..227567bb 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -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 &_inds, - std::vector &_sqrDists, std::vector &_interpolators1, std::vector &_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 &_cloud, pcl::PointCloud &_zSlice, std::vector &_zSliceInds, @@ -723,11 +721,15 @@ bool ScienceSensorsSystemPrivate::comparePclPoints( ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::FindTrilinearInterpolators( pcl::PointXYZ &_pt, - std::vector &_inds, - std::vector &_sqrDists, std::vector &_interpolators1, std::vector &_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) @@ -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 zSlice1, zSlice2; @@ -764,79 +753,31 @@ void ScienceSensorsSystemPrivate::FindTrilinearInterpolators( std::vector interpolatorInds1, interpolatorInds2; std::vector 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 cloudExceptZSlice1; - std::vector 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 inds2; - std::vector sqrDists2; - std::vector 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; } @@ -844,7 +785,8 @@ void ScienceSensorsSystemPrivate::FindTrilinearInterpolators( ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::CreateDepthSlice( - float _depth, + float _min_depth, + float _max_depth, pcl::PointCloud &_cloud, pcl::PointCloud &_zSlice, std::vector &_zSliceInds, @@ -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 @@ -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 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 "