diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index 8a9e8fdb..dd2d28c0 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -317,6 +317,18 @@ if(BUILD_TESTING) lrauv_range_bearing_request lrauv_range_bearing_response) gtest_discover_tests(test_range_bearing) + + add_executable(UNIT_Interpolation_TEST src/Interpolation_TEST.cc) + target_include_directories(UNIT_Interpolation_TEST + PUBLIC ${PCL_INCLUDE_DIRS}) + target_link_libraries(UNIT_Interpolation_TEST + PUBLIC gtest_main + PRIVATE + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-common${IGN_COMMON_VER}::profiler + ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} + ${PCL_LIBRARIES}) + gtest_discover_tests(UNIT_Interpolation_TEST) endif() #============================================================================ diff --git a/lrauv_ignition_plugins/data/interpolation_prism.csv b/lrauv_ignition_plugins/data/interpolation_prism.csv new file mode 100644 index 00000000..f013efd5 --- /dev/null +++ b/lrauv_ignition_plugins/data/interpolation_prism.csv @@ -0,0 +1,10 @@ +elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec +0,-0.00003,0.00003,0,0,0,300,-1,0 +0,-0.00006,0.00003,0,0,0,300,-1,0 +0,-0.00003,0.00006,0,0,0,300,-1,0 +0,-0.00006,0.00006,0,0,0,300,-1,0 +0,-0.00003,0.00003,2,0,0,100,-1,0 +0,-0.00006,0.00003,2,0,0,100,-1,0 +0,-0.00003,0.00006,2,0,0,100,-1,0 +0,-0.00006,0.00006,2,0,0,100,-1,0 +0,1,1,100,0,0,NaN,0,0 diff --git a/lrauv_ignition_plugins/data/interpolation_test.csv b/lrauv_ignition_plugins/data/interpolation_test.csv index 27d4e06f..5d2ee639 100644 --- a/lrauv_ignition_plugins/data/interpolation_test.csv +++ b/lrauv_ignition_plugins/data/interpolation_test.csv @@ -4,11 +4,11 @@ elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_tempe 0,-0.00003,-0.00003,0,0,0,100,-1,0 0,0.00000,-0.00003,0,0,0,100,-1,0 0,0.00003,-0.00003,0,0,0,100,-1,0 -0,-0.00009,-0.00003,1,0,0,300,-1,0 -0,-0.00006,-0.00003,1,0,0,300,-1,0 -0,-0.00003,-0.00003,1,0,0,300,-1,0 -0,0.00000,-0.00003,1,0,0,300,-1,0 -0,0.00003,-0.00003,1,0,0,300,-1,0 +0,-0.00009,-0.00003,2,0,0,300,-1,0 +0,-0.00006,-0.00003,2,0,0,300,-1,0 +0,-0.00003,-0.00003,2,0,0,300,-1,0 +0,0.00000,-0.00003,2,0,0,300,-1,0 +0,0.00003,-0.00003,2,0,0,300,-1,0 0,-0.00009,0.00000,0,0,0,200,-1,0 0,-0.00006,0.00000,0,0,0,200,-1,0 0,-0.00003,0.00000,0,0,0,200,-1,0 @@ -21,8 +21,18 @@ elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_tempe 0,-0.00003,0.00003,0,0,0,300,-1,0 0,0.00000,0.00003,0,0,0,300,-1,0 0,0.00003,0.00003,0,0,0,300,-1,0 -0,-0.00009,0.00003,1,0,0,100,-1,0 -0,-0.00006,0.00003,1,0,0,100,-1,0 -0,-0.00003,0.00003,1,0,0,100,-1,0 -0,0.00000,0.00003,1,0,0,100,-1,0 -0,0.00003,0.00003,1,0,0,100,-1,0 +0,-0.00009,0.00003,2,0,0,100,-1,0 +0,-0.00006,0.00003,2,0,0,100,-1,0 +0,-0.00003,0.00003,2,0,0,100,-1,0 +0,0.00000,0.00003,2,0,0,100,-1,0 +0,0.00003,0.00003,2,0,0,100,-1,0 +0,-0.00009,0.00006,0,0,0,300,-1,0 +0,-0.00006,0.00006,0,0,0,300,-1,0 +0,-0.00003,0.00006,0,0,0,300,-1,0 +0,0.00000,0.00006,0,0,0,300,-1,0 +0,0.00003,0.00006,0,0,0,300,-1,0 +0,-0.00009,0.00006,2,0,0,100,-1,0 +0,-0.00006,0.00006,2,0,0,100,-1,0 +0,-0.00003,0.00006,2,0,0,100,-1,0 +0,0.00000,0.00006,2,0,0,100,-1,0 +0,0.00003,0.00006,2,0,0,100,-1,0 diff --git a/lrauv_ignition_plugins/src/Interpolation.hh b/lrauv_ignition_plugins/src/Interpolation.hh new file mode 100644 index 00000000..eaee7549 --- /dev/null +++ b/lrauv_ignition_plugins/src/Interpolation.hh @@ -0,0 +1,1287 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#ifndef TETHYS_INTERPOLATION_HH_ +#define TETHYS_INTERPOLATION_HH_ + +#include +#include + +#include +#include +#include + +namespace tethys +{ +class InterpolationPrivate; + +/// \brief Interpolation methods +enum InterpolationMethod +{ + TRILINEAR, + BARYCENTRIC +}; + +/// \brief Algorithms for data interpolation +class Interpolation +{ + /// \brief Constructor + public: Interpolation(InterpolationMethod _method=TRILINEAR); + + /// \brief Set interpolation method + /// \param[in] _method Number from enum interpolation + public: void SetMethod(InterpolationMethod _method); + + /// \brief Get interpolation method + /// \returns Interpolation method in enum + public: InterpolationMethod Method(); + + /// \brief Find XYZ locations of points in the two closest z slices to + /// interpolate among. + /// \param[in] _cloud Spatial locations of existing data + /// \param[in] _queryPt Location in space to interpolate for + /// \param[in] _nnIdx Index of 1st nearest neighbor in the cloud to _queryPt. + /// Only the z of the point is used, to look for the first z slice in the + /// data. + /// \param[in] _spatialRes Spatial resolution to create an octree for the + /// search + /// \param[out] _interpolatorInds1 Indices of points in fist z slice + /// \param[out] _interpolators1 XYZ points on a z slice to interpolate among + /// \param[out] _interpolatorInds2 Indices of points in second z slice + /// \param[out] _interpolators2 XYZ points on a second z slice to interpolate + /// among + /// \param[in] _k Number of nearest neighbors. Default to 4, for trilinear + /// interpolation between two z slices of 4 points per slice. + public: void FindTrilinearInterpolators( + pcl::PointCloud &_cloud, + pcl::PointXYZ &_queryPt, + int _nnIdx, + float _spatialRes, + std::vector &_interpolatorInds1, + std::vector &_interpolators1, + std::vector &_interpolatorInds2, + std::vector &_interpolators2, + int _k=4); + + /// \brief Interpolate among existing science data to output an estimated + /// reading at the current sensor location. + /// \param[in] _p Position to interpolate for + /// \param[in] _xyzs XYZ coordinates of existing data locations + /// \param[in] _values Data values at the locations + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float InterpolateData( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + /// \brief Return general debug flag + /// \return Whether general debug flag is on + public: bool Debug(); + + /// \brief Set general debug flag + public: void SetDebug(bool debug); + + /// \brief Return debug flag for math calculations + /// \return Whether math debug flag is on + public: bool DebugMath(); + + /// \brief Set math debug flag + public: void SetDebugMath(bool debug); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; +}; + +class InterpolationPrivate +{ + /// \brief Constructor + public: InterpolationPrivate(InterpolationMethod _method=TRILINEAR); + + ////////////////////////////////// + // Trilinear interpolation + + /// \brief Comparison function for std::set_difference(). + /// Comparison between points is arbitrary. This function is only used for + /// set operations, not for literal sorting. + public: bool comparePclPoints( + const pcl::PointXYZ &a, + const pcl::PointXYZ &b); + + /// \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] _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 _zSlice in the original + /// point cloud _points + /// \param[in] _tol Tolerance around _depth to include in the slice + /// \param[in] _invert Invert the filter. Keep everything except the z slice. + public: void CreateDepthSlice( + float _depth, + pcl::PointCloud &_cloud, + pcl::PointCloud &_zSlice, + std::vector &_zSliceInds, + float _tol=1e-6, + bool _invert=false); + + /// \brief Create an octree from a point cloud, and search for _k nearest + /// neighbors. + /// \param[in] _searchPt Location in space to search from + /// \param[in] _cloud Point cloud to search in + /// \param[in] _spatialRes Spatial resolution of the octree + /// \param[out] _nbrInds Result of octree search, indices of points. + /// \param[out] _nbrSqrDists Result of octree search, distances. + /// \param[out] _nbrs XYZ of the k nearest neighbors found. + /// \param[in] _k Number of nearest neighbors. Default to 4, for trilinear + /// interpolation between two z slices of 4 points per slice. + public: void CreateAndSearchOctree( + pcl::PointXYZ &_searchPt, + pcl::PointCloud &_cloud, + float _spatialRes, + std::vector &_nbrInds, + std::vector &_nbrSqrDists, + std::vector &_nbrs, + int _k=4); + + /// \brief Trilinear interpolation for a point inside a prism, given 8 + /// verticies of the prism. Suitable for data laid out in a rectangular grid. + /// Otherwise, use a different interpolation method, e.g. barycentric. + /// If the points must be on different z slices, try the hybrid + /// BaryLinearInterpolate(). + /// \param[in] _p Position to interpolate for + /// \param[in] _xyzs XYZ coordinates of 8 vertices of a prism + /// \param[in] _values Data values at the 8 vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float TrilinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + ////////////////////////////////// + // Barycentric interpolation + + /// \brief Barycentric interpolation in 3D, given 4 points on any arbitrary + /// tetrahedra. + /// \param[in] _p Position within the tetrahedra to interpolate for + /// \param[in] _xyzs n x 3. XYZ coordinates of 4 vertices of a tetrahedra + /// \param[in] _values Data values at the 4 vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + /// \brief Barycentric interpolation in 2D, given 4 points on a plane. Finds + /// 3 points on a triangle within which the query point lies, then + /// interpolates using them. + /// \param[in] _p Position within the triangle to interpolate for + /// \param[in] _xys n x 2. XY coordinates of 3 vertices of a triangle + /// \param[in] _values Data values at the 3 vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const Eigen::Vector2f &_p, + const Eigen::Matrix &_xys, + const std::vector &_values); + + /// \brief 1D linear interpolation, given 4 points on a line. Finds 2 points + /// on a segment within which the query point lies, then interpolates using + /// them. + /// \param[in] _p Position to interpolate for + /// \param[in] _xs Positions to interpolate from + /// \param[in] _values Data values at the positions to interpolate from + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const float &_p, + const Eigen::VectorXf &_xs, + const std::vector &_values); + + /// \brief Sort vector, store indices to original vector after sorting. + /// Original vector unchanged. + /// \param[in] _v Vector to sort + /// \param[out] _idx Indices of original vector in sorted vector + public: template + void SortIndices( + const std::vector &_v, + std::vector &_idx); + + ////////////////////////////////// + // Hybrid interpolation. 2D triangle barycentric interpolation on 2 z slices, + // then linear interpolation. + + /// \brief Two 2D barycentric interpolations on two z slices, then a linaer + /// interpolation between the two intermediate results. + /// \param[in] _p Position within the tetrahedra to interpolate for + /// \param[in] _xyzs n x 8. XYZ coordinates of 2 sets of 4 vertices, each + /// set on a different z slice. [0]-[3] is first slice, [4]-[7] is 2nd slice. + /// \param[in] _values Data values at the vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BaryLinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + ////////////////////////////////// + /// Utility + + /// \brief Debug printouts for interpolation. Will keep around at least until + /// interpolation is stable. + public: bool debugGeneral = true; + + /// \brief Debug printouts for interpolation math. Will keep around at least + /// until interpolation is stable. + public: bool debugMath = false; + + /// \brief Interpolation method + public: InterpolationMethod method = TRILINEAR; +}; + +///////////////////////////////////////////////// +Interpolation::Interpolation(InterpolationMethod _method) + : dataPtr(std::make_unique(_method)) +{ +} + +///////////////////////////////////////////////// +InterpolationMethod Interpolation::Method() +{ + return this->dataPtr->method; +} + +///////////////////////////////////////////////// +void Interpolation::SetMethod(InterpolationMethod _method) +{ + switch (_method) + { + case TRILINEAR: + ignmsg << "Interpolation method set to TRILINEAR" << std::endl; + break; + case BARYCENTRIC: + ignmsg << "Interpolation method set to BARYCENTRIC" << std::endl; + break; + default: + ignerr << "Invalid interpolation method: " << _method + << ". Method not modified."<< std::endl; + return; + } + + this->dataPtr->method = _method; +} + +///////////////////////////////////////////////// +void Interpolation::FindTrilinearInterpolators( + pcl::PointCloud &_cloud, + pcl::PointXYZ &_queryPt, + int _nnIdx, + float _spatialRes, + std::vector &_interpolatorInds1, + std::vector &_interpolators1, + std::vector &_interpolatorInds2, + std::vector &_interpolators2, + int _k) +{ + IGN_PROFILE_THREAD_NAME("Interpolation FindTrilinearInterpolators"); + IGN_PROFILE("Interpolation::FindTrilinearInterpolators"); + + // Initialize return parameters + _interpolatorInds1.clear(); + _interpolators1.clear(); + _interpolatorInds2.clear(); + _interpolators2.clear(); + + // Sanity checks + // Point cloud not populated + if (_cloud.size() == 0) + { + return; + } + if (_nnIdx < 0) + { + ignwarn << "FindTrilinearInterpolators(): Invalid neighbor index (" + << _nnIdx + << "). No neighbors to use for slice search. Returning NaN." + << std::endl; + return; + } + + // Two slices of different depth z values + pcl::PointCloud zSlice1, zSlice2; + // Indices of points in the z slices + std::vector zSliceInds1, zSliceInds2; + + // Distances of neighboring points in the search results + // 4 above, 4 below + std::vector interpolatorSqrDists1, interpolatorSqrDists2; + + // Step 1: 1st NN, in its z slice, search for 4 NNs + + // Get z of 1st nearest neighbor + float nnZ = _cloud.at(_nnIdx).z; + + // Debug output + if (this->dataPtr->debugGeneral) + igndbg << _cloud.size() + << " points in full cloud" << std::endl; + + // Create a sub-cloud containing just the z slice + this->dataPtr->CreateDepthSlice(nnZ, _cloud, zSlice1, + zSliceInds1); + if (this->dataPtr->debugGeneral) + { + igndbg << "1st nn (" + << _cloud.at(_nnIdx).x << ", " + << _cloud.at(_nnIdx).y << ", " + << _cloud.at(_nnIdx).z << "), " + << "idx " << _nnIdx + << ", z slice " << zSlice1.points.size() << " points" << std::endl; + + // igndbg << "FindTrilinearInterpolators(): 1st z slice has indices: " + // << std::endl; + // for (int i = 0; i < zSliceInds1.size(); ++i) + // igndbg << zSliceInds1[i] << " " << std::endl; + } + + // Search in z slice for 4 nearest neighbors in this slice + std::vector interpolatorInds1NewCloud; + this->dataPtr->CreateAndSearchOctree(_queryPt, zSlice1, _spatialRes, + interpolatorInds1NewCloud, interpolatorSqrDists1, _interpolators1, _k); + if (interpolatorInds1NewCloud.size() < _k || + interpolatorSqrDists1.size() < _k) + { + ignwarn << "Could not find enough neighbors in 1st slice z = " << nnZ + << " for trilinear interpolation." << std::endl; + return; + } + + // Map back to the indices in the original point cloud for returning + _interpolatorInds1.clear(); + for (int i = 0; i < interpolatorInds1NewCloud.size(); ++i) + { + _interpolatorInds1.push_back(zSliceInds1[interpolatorInds1NewCloud[i]]); + } + + // Step 2: exclude z slice of 1st NN from further searches. + + // Maps from indices in the new point cloud with 1st z slice removed, to + // indices in the original point cloud. This is needed for returning + // second set of interpolator indices to map back to the original cloud. + std::vector newToOldInds; + + // Create a sub-cloud without 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; + + // Convert input indices to PCL type + pcl::PointIndices::Ptr zSliceInds1pcl(new pcl::PointIndices()); + zSliceInds1pcl->indices = zSliceInds1; + + // Remove all points in the z slice of the 1st NN, so that the 2nd NN will be + // found in another z slice. + pcl::ExtractIndices extract; + extract.setInputCloud(_cloud.makeShared()); + extract.setIndices(zSliceInds1pcl); + extract.setNegative(true); + extract.filter(cloudExceptZSlice1); + extract.filter(newToOldInds); + + if (this->dataPtr->debugGeneral) + { + igndbg << "Excluding 1st nn z slice. Remaining cloud has " + << cloudExceptZSlice1.points.size() << " points" << std::endl; + + // igndbg << "FindTrilinearInterpolators(): cloud except 1st z slice has " + // << "indices:" << std::endl; + // for (int i = 0; i < newToOldInds.size(); ++i) + // igndbg << newToOldInds[i] << " " << 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->dataPtr->CreateAndSearchOctree(_queryPt, cloudExceptZSlice1, + _spatialRes, 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 + + // Create a sub-sub-cloud containing just the z slice + this->dataPtr->CreateDepthSlice(nnZ2, cloudExceptZSlice1, zSlice2, + zSliceInds2); + if (this->dataPtr->debugGeneral) + { + igndbg << "2nd nn (" + << _cloud.at(nnIdx2).x << ", " + << _cloud.at(nnIdx2).y << ", " + << _cloud.at(nnIdx2).z << "), " + << "idx " << nnIdx2 << ", dist " << sqrt(minDist2) + << ", z slice " << zSlice2.points.size() << " points" << std::endl; + + // igndbg << "FindTrilinearInterpolators(): 2nd z slice has indices: " + // << std::endl; + // for (int i = 0; i < zSliceInds2.size(); ++i) + // igndbg << newToOldInds[zSliceInds2[i]] << " " << std::endl; + } + + // Search in z slice of 1st NN for 4 nearest neighbors in this slice + std::vector interpolatorInds2NewCloud; + this->dataPtr->CreateAndSearchOctree(_queryPt, zSlice2, _spatialRes, + interpolatorInds2NewCloud, interpolatorSqrDists2, _interpolators2, _k); + if (interpolatorInds2NewCloud.size() < _k || + interpolatorSqrDists2.size() < _k) + { + ignwarn << "Could not find enough neighbors in 2nd slice z = " << nnZ2 + << " for trilinear interpolation." << std::endl; + return; + } + + // Map back to the indices in the original point cloud for returning + _interpolatorInds2.clear(); + for (int i = 0; i < interpolatorInds2NewCloud.size(); ++i) + { + _interpolatorInds2.push_back( + newToOldInds[zSliceInds2[interpolatorInds2NewCloud[i]]]); + // igndbg << "interpolatorInds2NewCloud[i]: " + // << interpolatorInds2NewCloud[i] << std::endl; + // igndbg << "zSliceInds2[interpolatorInds2NewCloud[i]]: " + // << zSliceInds2[interpolatorInds2NewCloud[i]] << std::endl; + // igndbg << "newToOldInds[zSliceInds2[interpolatorInds2NewCloud[i]]]: " + // << newToOldInds[zSliceInds2[interpolatorInds2NewCloud[i]]] << std::endl; + } + + // if (this->dataPtr->debugGeneral) + // { + // igndbg << "FindTrilinearInterpolators(): result indices:" << std::endl; + // for (int i = 0; i < _interpolatorInds1.size(); ++i) + // igndbg << _interpolatorInds1[i] << " " << std::endl; + // for (int i = 0; i < _interpolatorInds2.size(); ++i) + // igndbg << _interpolatorInds2[i] << " " << std::endl; + // } +} + +///////////////////////////////////////////////// +float Interpolation::InterpolateData( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + if (this->dataPtr->method == TRILINEAR) + { + return this->dataPtr->TrilinearInterpolate(_p, _xyzs, _values); + } + else if (this->dataPtr->method == BARYCENTRIC) + { + return this->dataPtr->BarycentricInterpolate(_p, _xyzs, _values); + } + else + { + ignerr << "method value invalid. " + << "Choose a valid interpolation method." << std::endl; + return std::numeric_limits::quiet_NaN(); + } +} + +///////////////////////////////////////////////// +bool Interpolation::Debug() +{ + return this->dataPtr->debugGeneral; +} + +///////////////////////////////////////////////// +void Interpolation::SetDebug(bool debug) +{ + this->dataPtr->debugGeneral = debug; +} + +///////////////////////////////////////////////// +bool Interpolation::DebugMath() +{ + return this->dataPtr->debugMath; +} + +///////////////////////////////////////////////// +void Interpolation::SetDebugMath(bool debug) +{ + this->dataPtr->debugMath = debug; +} + +///////////////////////////////////////////////// +InterpolationPrivate::InterpolationPrivate(InterpolationMethod _method) + : method(_method) +{ +} + +///////////////////////////////////////////////// +bool InterpolationPrivate::comparePclPoints( + const pcl::PointXYZ &a, + const pcl::PointXYZ &b) +{ + // Comparison between points is arbitrary. This function is only used for + // set operations, not for literal sorting. + return a.x < b.x && a.y < b.y && a.z < b.z; +} + +///////////////////////////////////////////////// +void InterpolationPrivate::CreateDepthSlice( + float _depth, + pcl::PointCloud &_cloud, + pcl::PointCloud &_zSlice, + std::vector &_zSliceInds, + float _tol, + bool _invert) +{ + // Separate a z slice, i.e. points with z equal to that of 1st NN + // Pass in _invert=true to get indices of removed points + pcl::PassThrough passThruFilter = + pcl::PassThrough(true); + passThruFilter.setInputCloud(_cloud.makeShared()); + passThruFilter.setNegative(_invert); + passThruFilter.setFilterFieldName("z"); + passThruFilter.setFilterLimits(_depth - _tol, _depth + _tol); + passThruFilter.filter(_zSlice); + passThruFilter.filter(_zSliceInds); +} + +///////////////////////////////////////////////// +void InterpolationPrivate::CreateAndSearchOctree( + pcl::PointXYZ &_searchPt, + pcl::PointCloud &_cloud, + float _spatialRes, + std::vector &_nbrInds, + std::vector &_nbrSqrDists, + std::vector &_nbrs, + int _k) +{ + // Initialize return value populated by hand + _nbrs.clear(); + + // Create octree for cloud + auto octree = pcl::octree::OctreePointCloudSearch( + _spatialRes); + octree.setInputCloud(_cloud.makeShared()); + octree.addPointsFromInputCloud(); + + // Search in the depth slice to find 4 closest neighbors + if (octree.getLeafCount() > 0) + { + if (octree.nearestKSearch( + _searchPt, _k, _nbrInds, _nbrSqrDists) <= 0) + { + ignwarn << "No data found near search point " << _searchPt + << std::endl; + return; + } + else + { + igndbg << "Found " << _nbrInds.size() << " neighbors." + << std::endl; + + for (std::size_t i = 0; i < _nbrInds.size(); ++i) + { + pcl::PointXYZ nbrPt = _cloud.at(_nbrInds[i]); + _nbrs.push_back(nbrPt); + + igndbg << "Neighbor at (" + << std::round(nbrPt.x * 1000.0) / 1000.0 << ", " + << std::round(nbrPt.y * 1000.0) / 1000.0 << ", " + << std::round(nbrPt.z * 1000.0) / 1000.0 << "), " + << "distance " << sqrt(_nbrSqrDists[i]) << " m" << std::endl; + } + } + } + else + { + ignwarn << "Zero points in this octree." << std::endl; + } +} + +///////////////////////////////////////////////// +float InterpolationPrivate::TrilinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + IGN_PROFILE_THREAD_NAME("Interpolation TrilinearInterpolate"); + IGN_PROFILE("InterpolationPrivate::TrilinearInterpolate"); + + // Sanity check: Must have 8 points, 4 above, 4 below. + if (_xyzs.rows() != 8) + { + ignerr << "Size of interpolators invalid (" << _xyzs.rows() << "). " + << "Need 8 points in a rectangular prism. " + << "Cannot perform trilinear interpolation." << std::endl; + return std::numeric_limits::quiet_NaN(); + } + + // A rectangular prism can be represented by two pairs of 3D coordinates, + // (x0, y0, z0) and (x1, y1, z1), which are diagonal vertices on the prism. + // Extract 2 diagonal vertices to represent the rectangular prism, assuming + // the points are corners of a prism (assumption checked after extracting). + // Corner of minimum x y z + auto v000 = Eigen::Vector3f( + _xyzs.col(0).minCoeff(), + _xyzs.col(1).minCoeff(), + _xyzs.col(2).minCoeff()); + // Corner of maximum x y z + auto v111 = Eigen::Vector3f( + _xyzs.col(0).maxCoeff(), + _xyzs.col(1).maxCoeff(), + _xyzs.col(2).maxCoeff()); + // 6 remaining vertices of prism + Eigen::Vector3f v001, v010, v011, v100, v101, v110; + + igndbg << "Trilinear interpolation min vert v000: " + << v000(0) << ", " << v000(1) << ", " << v000(2) << std::endl; + igndbg << "Trilinear interpolation max vert v111: " + << v111(0) << ", " << v111(1) << ", " << v111(2) << std::endl; + + // Data values at the vertices + // Define explicitly for readability in interpolation equations later + float d000, d001, d010, d011, d100, d101, d110, d111; + + // Tolerance below which to consider two values equal + const double TOLERANCE = 1e-6; + + // Find ordering of corners in prism, assign data to ordered corners. + // Each vertex, if really on the corner of a rectangular prism, must share + // at least one component of its xyz coordinates with v000, and the other + // components are shared with v111. + // The actual vertices are v000, v001, v010, v011, v100, v101, v110, v111. + // Example: v100 = (v111_x, v000_y, v000_z). Similar for others. + for (int r = 0; r < _xyzs.rows(); ++r) + { + /// 000 or [0] + if (fabs(_xyzs(r, 0) - v000(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v000(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v000(2)) <= TOLERANCE) + { + d000 = _values[r]; + } + // 001 or [1] + else if (fabs(_xyzs(r, 0) - v000(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v000(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v111(2)) <= TOLERANCE) + { + v001 = _xyzs.row(r); + d001 = _values[r]; + } + // 010 or [2] + else if (fabs(_xyzs(r, 0) - v000(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v111(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v000(2)) <= TOLERANCE) + { + v010 = _xyzs.row(r); + d010 = _values[r]; + } + // 011 or [3] + else if (fabs(_xyzs(r, 0) - v000(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v111(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v111(2)) <= TOLERANCE) + { + v011 = _xyzs.row(r); + d011 = _values[r]; + } + // 100 or [4] + else if (fabs(_xyzs(r, 0) - v111(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v000(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v000(2)) <= TOLERANCE) + { + v100 = _xyzs.row(r); + d100 = _values[r]; + } + // 101 or [5] + else if (fabs(_xyzs(r, 0) - v111(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v000(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v111(2)) <= TOLERANCE) + { + v101 = _xyzs.row(r); + d101 = _values[r]; + } + // 110 or [6] + else if (fabs(_xyzs(r, 0) - v111(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v111(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v000(2)) <= TOLERANCE) + { + v110 = _xyzs.row(r); + d110 = _values[r]; + } + // 111 or [7] + else if (fabs(_xyzs(r, 0) - v111(0)) <= TOLERANCE && + fabs(_xyzs(r, 1) - v111(1)) <= TOLERANCE && + fabs(_xyzs(r, 2) - v111(2)) <= TOLERANCE) + { + d111 = _values[r]; + } + else + { + // Enforce that the points are vertices of a rectangular prism. + // Otherwise does not satisfy trilinear interpolation requirements. + ignwarn << "Trilinear interpolation: " + << "Suspect 8 input points not on prism. Vertex " << r << " (" + << std::round(_xyzs(r, 0) * 1000.0) / 1000.0 << ", " + << std::round(_xyzs(r, 1) * 1000.0) / 1000.0 << ", " + << std::round(_xyzs(r, 2) * 1000.0) / 1000.0 + << ") not within tolerance (" << TOLERANCE + << ") of any of 8 vertices of rectangular prism. " + << "Using hybrid bary-linear interpolation instead." << std::endl; + return this->BaryLinearInterpolate(_p, _xyzs, _values); + } + } + + if (this->debugGeneral) + igndbg << "Trilinear interpolation, starting with 8 points: " + << d000 << ", " << d001 << ", " << d010 << ", " << d011 << ", " + << d100 << ", " << d101 << ", " << d110 << ", " << d111 + << std::endl; + + // Trilinear interpolation using 8 corners of a rectangular prism + // Implements https://en.wikipedia.org/wiki/Trilinear_interpolation + + // Ratio describing where the target point is within the cube + float dx = (_p(0) - v000(0)) / (v111(0) - v000(0)); + float dy = (_p(1) - v000(1)) / (v111(1) - v000(1)); + float dz = (_p(2) - v000(2)) / (v111(2) - v000(2)); + + // By definition of trilinear interpolation, the order you interpolate among + // xyz does not matter. They will give the same result. + // Interpolate along x, to find where target point is wrt each pair of x's. + // For 8 points, there are 4 pairs of x's. + float d00 = d000 * (1 - dx) + d100 * dx; + float d01 = d001 * (1 - dx) + d101 * dx; + float d10 = d010 * (1 - dx) + d110 * dx; + float d11 = d011 * (1 - dx) + d111 * dx; + + if (this->debugGeneral) + igndbg << "Trilinear interpolation, 4 intermediate results from 8 points: " + << d00 << ", " << d01 << ", " << d10 << ", " << d11 << std::endl; + + // Interpolate along y + // For 4 interpolated points on x above, there are 2 pairs of y's + float d0 = d00 * (1 - dy) + d10 * dy; + float d1 = d01 * (1 - dy) + d11 * dy; + + if (this->debugGeneral) + igndbg << "Trilinear interpolation, 2 intermediate results from 4 points: " + << d0 << ", " << d1 << std::endl; + + // Interpolate along z + // For 2 interpolated points on y above, there is only 1 z. + // This arrives at the interpolated value at the target xyz position. + float d = d0 * (1 - dz) + d1 * dz; + + if (this->debugGeneral) + igndbg << "Trilinear interpolation result: " << d << std::endl; + + return d; +} + +///////////////////////////////////////////////// +float InterpolationPrivate::BarycentricInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + IGN_PROFILE_THREAD_NAME("Interpolation BarycentricInterpolate"); + IGN_PROFILE("InterpolationPrivate::BarycentricInterpolate"); + + // Implemented from https://en.wikipedia.org/wiki/Barycentric_coordinate_system#Barycentric_coordinates_on_tetrahedra + + if (this->debugGeneral) + igndbg << "BarycentricInterpolate() 3D tetrahedron" << std::endl; + if (this->debugMath) + igndbg << "_p: " << std::endl << _p << std::endl; + + Eigen::Matrix3f T; + // Row 1 is x-coords: x1 - x4, x2 - x4, x3 - x4 + T << _xyzs(0, 0) - _xyzs(3, 0), + _xyzs(1, 0) - _xyzs(3, 0), + _xyzs(2, 0) - _xyzs(3, 0), + // Row 2 is y-coords: y1 - y4, y2 - y4, y3 - y4 + _xyzs(0, 1) - _xyzs(3, 1), + _xyzs(1, 1) - _xyzs(3, 1), + _xyzs(2, 1) - _xyzs(3, 1), + // Row 3 is z-coords: z1 - z4, z2 - z4, z3 - z4 + _xyzs(0, 2) - _xyzs(3, 2), + _xyzs(1, 2) - _xyzs(3, 2), + _xyzs(2, 2) - _xyzs(3, 2); + if (this->debugMath) + igndbg << "T: " << std::endl << T << std::endl; + + // Count number of rows in T that are all zeros + int zeroRowCount = 0; + bool rowIsZero[3] = {false, false, false}; + for (int r = 0; r < T.rows(); ++r) + { + if ((T.row(r).array().abs() < 1e-6).all()) + { + zeroRowCount++; + rowIsZero[r] = true; + } + } + + // If exactly 1 row of T is all zeros, then the points are in a 2D plane. + // 2D. Interpolate on a plane. Otherwise T inverse will result in nans. + if (zeroRowCount == 1) + { + if (this->debugGeneral) + igndbg << "4 points are on a plane. Using 2D barycentric interpolation " + "for a triangle." << std::endl; + + // Eliminate the constant axis + Eigen::Vector2f p2D; + Eigen::Matrix xyzs2D; + int nextCol = 0; + for (int r = 0; r < T.rows(); ++r) + { + if (!rowIsZero[r]) + { + // Populate the axes corresponding to nonzero rows of T. + // E.g. If row 1 of T is zeros, then points are on x-plane. Ignore + // x-coordinates, which are on column 1 of the original points matrix. + p2D(nextCol) = _p(r); + xyzs2D.col(nextCol) = _xyzs.col(r); + ++nextCol; + } + } + return this->BarycentricInterpolate(p2D, xyzs2D, _values); + } + // 1D. Interpolate on a line. Otherwise T inverse will result in nans. + else if (zeroRowCount == 2) + { + if (this->debugGeneral) + igndbg << "4 points are on a line. Using 1D interpolation." << std::endl; + + float p1D; + Eigen::VectorXf xyzs1D(_xyzs.rows()); + for (int r = 0; r < T.rows(); ++r) + { + // Only one row is non-zero + if (!rowIsZero[r]) + { + p1D = _p(r); + xyzs1D = _xyzs.col(r); + } + } + return this->BarycentricInterpolate(p1D, xyzs1D, _values); + } + // T is entirely zero. Then all points are at the same point. Take any value. + else if (zeroRowCount == 3) + { + if (this->debugGeneral) + igndbg << "4 points are at the exact same point. Arbitrarily selecting " + "one of their values as interpolation result." << std::endl; + return _values[0]; + } + + // r4 = (x4, y4, z4) + Eigen::Vector3f r4; + r4 << _xyzs(3, 0), _xyzs(3, 1), _xyzs(3, 2); + if (this->debugMath) + igndbg << "r4: " << std::endl << r4 << std::endl; + + // (lambda1, lambda2, lambda3) + Eigen::Vector3f lambda123 = T.inverse() * (_p - r4); + + if (this->debugMath) + igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; + + // lambda4 = 1 - lambda1 - lambda2 - lambda3 + float lambda4 = 1 - lambda123(0) - lambda123(1) - lambda123(2); + + if (this->debugGeneral) + igndbg << "Barycentric 3D lambda 1 2 3 4: " << lambda123(0) << ", " + << lambda123(1) << ", " + << lambda123(2) << ", " + << lambda4 << std::endl; + + // f(r) = lambda1 * f(r1) + lambda2 * f(r2) + lambda3 * f(r3) + float result = + lambda123(0) * _values[0] + + lambda123(1) * _values[1] + + lambda123(2) * _values[2] + + lambda4 * _values[3]; + + if (this->debugGeneral) + igndbg << "Barycentric 3D interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << result << std::endl; + + return result; +} + +///////////////////////////////////////////////// +float InterpolationPrivate::BarycentricInterpolate( + const Eigen::Vector2f &_p, + const Eigen::Matrix &_xys, + const std::vector &_values) +{ + if (this->debugGeneral) + igndbg << "BarycentricInterpolate() 2D triangle" << std::endl; + if (this->debugMath) + { + igndbg << "Query point (_p): " << std::endl << _p << std::endl; + igndbg << "Interpolating neighbors (_xys): " << std::endl << _xys << std::endl; + } + + // 2D case, consider inputs a triangle and use 2 x 2 matrix for T + Eigen::Matrix2f T(2, 2); + Eigen::Vector2f lastVert; + Eigen::Vector2f lambda12; + float lambda3; + + bool foundTriangle = false; + + // Eliminate the correct point, so that we have a triangle that the query + // point lies within. + for (int r = 0; r < _xys.rows(); ++r) + { + Eigen::Matrix xys3; + int nextRow = 0; + // Populate temp matrix with all points except current point (row) + for (int r2 = 0; r2 < xys3.rows(); ++r2) + { + if (r2 == r) + { + continue; + } + xys3.row(nextRow++) = _xys.row(r2); + } + if (this->debugMath) + igndbg << "xys3: " << std::endl << xys3 << std::endl; + + // Row 1: x1 - x3, x2 - x3 + T << xys3(0, 0) - xys3(2, 0), + xys3(1, 0) - xys3(2, 0), + // Row 2: y1 - y3, y2 - y3 + xys3(0, 1) - xys3(2, 1), + xys3(1, 1) - xys3(2, 1); + if (this->debugMath) + igndbg << "T: " << std::endl << T << std::endl; + + // lastVert = (x3, y3) + lastVert << xys3(2, 0), xys3(2, 1); + if (this->debugMath) + igndbg << "lastVert: " << std::endl << lastVert << std::endl; + + // (lambda1, lambda2) + lambda12 = T.inverse() * (_p - lastVert); + + if (this->debugMath) + igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; + + // lambda3 = 1 - lambda1 - lambda2 + lambda3 = 1 - lambda12(0) - lambda12(1); + + if (this->debugGeneral) + { + igndbg << "Barycentric 2D lambda 1 2 3: " << lambda12(0) << ", " + << lambda12(1) << ", " + << lambda3 << std::endl; + } + + // If all lambdas >= 0, then we found a triangle that the query point + // lies within. (A lambda would be negative if point is outside triangle) + if ((lambda12.array() >= 0).all() && lambda3 >= 0) + { + foundTriangle = true; + break; + } + } + + // If no valid triangle found, check if this is a degenerative case + if (!foundTriangle) + { + // Count number of rows in T that are all zeros, to detect degenerative case + int zeroRowCount = 0; + bool rowIsZero[2] = {false, false}; + for (int r = 0; r < T.rows(); ++r) + { + if ((T.row(r).array().abs() < 1e-6).all()) + { + zeroRowCount++; + rowIsZero[r] = true; + } + } + // If T has a row of zeros, degenerate to 1D + if (zeroRowCount == 1) + { + if (this->debugGeneral) + igndbg << "4 points are on a line. Using 1D interpolation." << std::endl; + + float p1D; + Eigen::VectorXf xys1D(_xys.rows()); + for (int r = 0; r < T.rows(); ++r) + { + // Only one row is non-zero + if (!rowIsZero[r]) + { + p1D = _p(r); + xys1D = _xys.col(r); + } + } + return this->BarycentricInterpolate(p1D, xys1D, _values); + } + } + + // f(r) = lambda1 * f(r1) + lambda2 * f(r2) + float result = + lambda12(0) * _values[0] + + lambda12(1) * _values[1] + + lambda3 * _values[2]; + + if (this->debugGeneral) + igndbg << "Barycentric 2D interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] + << " resulted in " << result << std::endl; + + return result; +} + +///////////////////////////////////////////////// +float InterpolationPrivate::BarycentricInterpolate( + const float &_p, + const Eigen::VectorXf &_xs, + const std::vector &_values) +{ + if (this->debugGeneral) + igndbg << "BarycentricInterpolate() 1D line" << std::endl; + if (this->debugMath) + { + igndbg << "_p: " << std::endl << _p << std::endl; + igndbg << "_xs: " << std::endl << _xs << std::endl; + } + + // If _p is equal to one of the points, just take the value of that point. + // This is to catch floating point errors if _p lies on one side of all + // points in _xs, but really equal to one of the endpoints. + if (((_xs.array() - _p).abs() < 1e-6).any()) + { + for (int i = 0; i < _xs.size(); ++i) + { + if (abs(_xs(i) - _p) < 1e-6) + { + if (this->debugGeneral) + igndbg << "_p lies on a neighbor. " + << "1D linear interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << _values[i] << std::endl; + return _values[i]; + } + } + } + + // If _p lies on one side of all points in _xs, then cannot interpolate. + if ((_xs.array() - _p < 0).all() || + (_xs.array() - _p > 0).all()) + { + ignwarn << "1D linear interpolation: query point lies on one side of all " + "interpolation points. Cannot interpolate." << std::endl; + return std::numeric_limits::quiet_NaN(); + } + + // Sort points and store the indices + std::vector xsSorted; + std::vector 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{-1}; + int gtPSortedIdx{-1}; + float ltPDist, gtPDist; + + // Get the two closest positions in _xs that _p lies between. + for (int i = 0; i < xsSorted.size() - 1; ++i) + { + // Two consecutive elements in the sorted vector, that the query point lies + // between, are the closest points to each side of the query point. + if (xsSorted[i] <= _p && _p <= xsSorted[i+1]) + { + ltPSortedIdx = i; + gtPSortedIdx = i + 1; + + ltPDist = _p - xsSorted[i]; + gtPDist = xsSorted[i+1] - _p; + + break; + } + } + + // 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->debugGeneral) + { + 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::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::quiet_NaN(); + } + + // Linear interpolation + float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx]; + + if (this->debugGeneral) + { + igndbg << "ltPWeight " << ltPWeight << " on value " << _values[ltPIdx] + << ", gtPWeight " << gtPWeight << " on value " << _values[gtPIdx] + << std::endl; + igndbg << "1D linear interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << result << std::endl; + } + + return result; +} + +///////////////////////////////////////////////// +template +void InterpolationPrivate::SortIndices( + const std::vector &_v, + std::vector &_idx) +{ + // From https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes + + // Initialize original index locations + _idx.resize(_v.size()); + std::iota(_idx.begin(), _idx.end(), 0); + + // 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 + std::stable_sort(_idx.begin(), _idx.end(), + [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];}); +} + +///////////////////////////////////////////////// +float InterpolationPrivate::BaryLinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + // Drop z dimension, since we are assuming triangles are on z slices + Eigen::Vector2f p2d; + p2d << _p(0), _p(1); + + if (_xyzs.rows() != 8 || _values.size() != 8) + { + ignerr << "BaryLinearInterpolate(): Unexpected number of neighbors (" + << _xyzs.rows() << " and " << _values.size() << "). " + << "Aborting interpolation." << std::endl; + return std::numeric_limits::quiet_NaN(); + } + + // Step 1 + // Interpolate among the 4 points in each z slice, to get 2 resulting points, + // at (x, y, z1) and (x, y, z2), where (x, y, z) is the query point, and z1 + // and z2 are the z's of the slices. + // Assume [0]-[3] is first slice, [4]-[7] is 2nd slice. + // 4 x 2. Drop z dimension + Eigen::Matrix xysSlice1 = _xyzs.block(0, 0, 4, 2); + Eigen::Matrix xysSlice2 = _xyzs.block(4, 0, 4, 2); + float valueSlice1 = this->BarycentricInterpolate(p2d, xysSlice1, _values); + float valueSlice2 = this->BarycentricInterpolate(p2d, xysSlice2, _values); + if (this->debugGeneral) + igndbg << "Hybrid bary-linear interpolation, " + << "2 barycentric interpolations on 2 z slices resulted in: " + << valueSlice1 << " and " << valueSlice2 << std::endl; + + // Assume [0]-[3] is first slice, [4]-[7] is 2nd slice. + float z1 = _xyzs(0, 2); + float z2 = _xyzs(4, 2); + float z = _p(2); + + // Step 2 + // Linear interpolation between the 2 intermediate points, (x, y, z1) and + // (x, y, z2). Interpolate linearly along z to get value at query point + // (x, y, z). + float dz1 = fabs(z1 - z); + float dz2 = fabs(z2 - z); + float dz1Weight = dz1 / (dz1 + dz2); + float dz2Weight = dz2 / (dz1 + dz2); + float result = dz1Weight * valueSlice1 + dz2Weight * valueSlice2; + if (this->debugGeneral) + igndbg << "Hybrid bary-linear interpolation, " + << "linear interpolation of 2 points on two z slices: " << result + << std::endl; + + if (this->debugGeneral) + { + igndbg << "Hybrid bary-linear interpolation of " << _values.size() + << " values:" << std::endl; + for (int i = 0; i < _values.size(); ++i) + igndbg << _values[i] << std::endl; + igndbg << "resulted in " << result << std::endl; + } + + return result; +} +} +#endif diff --git a/lrauv_ignition_plugins/src/Interpolation_TEST.cc b/lrauv_ignition_plugins/src/Interpolation_TEST.cc new file mode 100644 index 00000000..ad75797c --- /dev/null +++ b/lrauv_ignition_plugins/src/Interpolation_TEST.cc @@ -0,0 +1,643 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include + +#include + +#include "Interpolation.hh" + +namespace tethys +{ + +///////////////////////////////////////////////// +TEST(InterpolationTest, SetAndGetMethod) +{ + // Constructor using default arg + Interpolation interp_dft; + EXPECT_EQ(interp_dft.Method(), TRILINEAR); + + // Constructor using default arg explicitly + Interpolation interp1(TRILINEAR); + EXPECT_EQ(interp1.Method(), TRILINEAR); + + interp1.SetMethod(BARYCENTRIC); + EXPECT_EQ(interp1.Method(), BARYCENTRIC); + + // Constructor using alternative arg + Interpolation interp2(BARYCENTRIC); + EXPECT_EQ(interp2.Method(), BARYCENTRIC); + + interp2.SetMethod(TRILINEAR); + EXPECT_EQ(interp2.Method(), TRILINEAR); +} + +///////////////////////////////////////////////// +TEST(InterpolationTest, TrilinearSearchEightPointPrism) +{ + Interpolation interp(TRILINEAR); + EXPECT_EQ(interp.Method(), TRILINEAR); + + float spatialRes = 0.1f; + int k = 4; + + // Inputs to search function + pcl::PointCloud cloud; + + // Populate input cloud. 8-point prism + // A rectangle at z = 0 + cloud.points.push_back(pcl::PointXYZ(0, 0, 0)); + cloud.points.push_back(pcl::PointXYZ(1, 0, 0)); + cloud.points.push_back(pcl::PointXYZ(0, 1, 0)); + cloud.points.push_back(pcl::PointXYZ(1, 1, 0)); + // Same rectangle at z = -2 + cloud.points.push_back(pcl::PointXYZ(0, 0, -2)); + cloud.points.push_back(pcl::PointXYZ(1, 0, -2)); + cloud.points.push_back(pcl::PointXYZ(0, 1, -2)); + cloud.points.push_back(pcl::PointXYZ(1, 1, -2)); + // An outlier + cloud.points.push_back(pcl::PointXYZ(100, 100, -100)); + + ////////////////////////////////// + // Test normal case, query point inside a prism cell + + // Query point is almost centered in upper z slice, but slightly toward + // point [1] + pcl::PointXYZ queryPt(0.6, 0.3, 0); + // 1st NN is at index [1] + int nnIdx = 1; + + // Return values from trilinear search + std::vector interpInds1, interpInds2; + std::vector interps1, interps2; + + // Search function. Search neighbors to do trilinear interpolation among + interp.FindTrilinearInterpolators(cloud, queryPt, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + + // Check neighbors found by trilinear search + EXPECT_EQ(interpInds1.size(), k); + EXPECT_EQ(interps1.size(), k); + EXPECT_EQ(interpInds2.size(), k); + EXPECT_EQ(interps2.size(), k); + + // Indices of points in 1st z slice found + EXPECT_EQ(interpInds1[0], 1); + EXPECT_EQ(interpInds1[1], 0); + EXPECT_EQ(interpInds1[2], 3); + EXPECT_EQ(interpInds1[3], 2); + + // Indices of points in 2nd z slice found + EXPECT_EQ(interpInds2[0], 5); + EXPECT_EQ(interpInds2[1], 4); + EXPECT_EQ(interpInds2[2], 7); + EXPECT_EQ(interpInds2[3], 6); + + ////////////////////////////////// + // Test edge case: query point is outside the prism. Happens when query point + // is outside the boundaries of regions where data are available. + + // Move query point outside and above prism. (Nearest neighbor remains same) + pcl::PointXYZ queryPtAbove(queryPt.x, queryPt.y, 1); + interp.FindTrilinearInterpolators(cloud, queryPtAbove, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + // Check that it still finds two z slices, even though the point is not + // between the slices (expected behavior, can be improved in algorithm). + EXPECT_EQ(interpInds1.size(), k); + EXPECT_EQ(interps1.size(), k); + EXPECT_EQ(interpInds2.size(), k); + EXPECT_EQ(interps2.size(), k); + + ////////////////////////////////// + // Test invalid cases + + // Move query outside prism, to overlap with outlier + pcl::PointXYZ queryPtOutlier(100, 100, -100); + nnIdx = 8; + interp.FindTrilinearInterpolators(cloud, queryPtOutlier, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + // Check that it cannot find 2 z slices for query point + EXPECT_EQ(interpInds1.size(), 0); + + // Invalid index for nearest neighbor + interp.FindTrilinearInterpolators(cloud, queryPt, -1, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + EXPECT_EQ(interpInds1.size(), 0); + + // Remove a point in a z slice + pcl::PointCloud cloudShort = cloud; + cloudShort.points.erase(cloudShort.points.begin() + 0); + interp.FindTrilinearInterpolators(cloudShort, queryPt, 0, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + // Check that it cannot find 2 z slices for query point + EXPECT_EQ(interpInds1.size(), 0); + + // Empty cloud + pcl::PointCloud emptyCloud; + interp.FindTrilinearInterpolators(emptyCloud, queryPt, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + // Check the function clears return values + EXPECT_EQ(interpInds1.size(), 0); + EXPECT_EQ(interpInds2.size(), 0); + EXPECT_EQ(interps1.size(), 0); + EXPECT_EQ(interps2.size(), 0); + + // TODO Add case where point is to left or right of prism, outside prism, but + // is in between z slices +} + +///////////////////////////////////////////////// +TEST(InterpolationTest, TrilinearInterpolationInEightPointPrism) +{ + Interpolation interp(TRILINEAR); + EXPECT_EQ(interp.Method(), TRILINEAR); + + // Inputs to interpolation function + + // One point per row + Eigen::MatrixXf interpsEigen(8, 3); + interpsEigen << + // A rectangle at z = 0 + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 1, 1, 0, + // Same rectangle at z = -2 + 0, 0, -2, + 1, 0, -2, + 0, 1, -2, + 1, 1, -2; + + // Query point is almost centered, lying in upper z slice, but slightly toward + // point [1] + Eigen::Vector3f queryPtEigen(0.6, 0.3, 0); + + // Data to test interpolation between two z slices + std::vector data; + data.push_back(0.0f); + data.push_back(0.0f); + data.push_back(0.0f); + data.push_back(0.0f); + data.push_back(200.0f); + data.push_back(200.0f); + data.push_back(200.0f); + data.push_back(200.0f); + + // Interpolation function + float result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Query point at z = 0 should take on average value in z = 0 slice + EXPECT_EQ(result, 0); + + // Move query to z = -2, on z slice + queryPtEigen[2] = -2; + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Query point at z = -2 should take on average value in z = -2 slice + EXPECT_EQ(result, 200); + + // Move query to z = -1, midpoint between z slices + queryPtEigen[2] = -1; + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Query point at z = -1 should take on average value of z = 0 and z = -2 + // slices + EXPECT_EQ(result, 100); + + // Move query to z = -0.5, a fraction between z slices + queryPtEigen[2] = -0.5; + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + EXPECT_NEAR(result, 50, 1); + + // Move query to z = -1.3, a fraction between z slices + queryPtEigen[2] = -1.3; + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + EXPECT_NEAR(result, 130, 1); + + // Data to test query point overlapping with data point + std::vector dataUnique; + dataUnique.push_back(100.0f); + dataUnique.push_back(200.0f); + dataUnique.push_back(300.0f); + dataUnique.push_back(400.0f); + dataUnique.push_back(500.0f); + dataUnique.push_back(600.0f); + dataUnique.push_back(700.0f); + dataUnique.push_back(800.0f); + + // Move query to overlap exactly with each data point + for (int i = 0; i < interpsEigen.rows(); ++i) + { + queryPtEigen = interpsEigen.row(i); + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Query point should take on value exactly at the point it overlaps with + EXPECT_EQ(result, data[i]); + } + + // Test invalid input for which function returns NaN + interpsEigen.resize(9, 3); + result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + EXPECT_TRUE(std::isnan(result)); +} + +///////////////////////////////////////////////// +// Happens when trilinear interpolation assumption of points being corners of +// a prism is not satisfied, falls back to hybrid "barylinear" interpolation, +// i.e. linear interpolation between 3D non-prism quads in two z slices, or the +// degenerative cases of a 2D triangle, or a 1D line segment. +// This tests the 3D case. +/* TODO: Not passing yet. Need to fix _values indexing +TEST(InterpolationTest, TrilinearFallbackToHybridBarylinear3D) +{ + ignition::common::Console::SetVerbosity(4); + + Interpolation interp(TRILINEAR); + EXPECT_EQ(interp.Method(), TRILINEAR); + + interp.SetDebug(true); + interp.SetDebugMath(false); + + // Inputs to search function + float spatialRes = 0.1f; + int k = 4; + pcl::PointCloud cloud; + + // Populate input cloud + // Top-down view: + // y + // | + // 1 * * * + // 0 --*----*q---*- x + // | + // -1 0 1 + // z view: + // z + // 0| * * (4 points) + // | q + // -1| + // -2| * * (4 points) + // + // A rectangle at z = 0 + cloud.points.push_back(pcl::PointXYZ( 0, 0, 0)); + cloud.points.push_back(pcl::PointXYZ( 1, 0, 0)); + cloud.points.push_back(pcl::PointXYZ( 0, 1, 0)); + cloud.points.push_back(pcl::PointXYZ( 1, 1, 0)); + // Same rectangle at z = -2 + cloud.points.push_back(pcl::PointXYZ( 0, 0, -2)); + cloud.points.push_back(pcl::PointXYZ(-1, 0, -2)); + cloud.points.push_back(pcl::PointXYZ( 0, 1, -2)); + cloud.points.push_back(pcl::PointXYZ(-1, 1, -2)); + + // Make a query point that is the general 3D quad case, but not prism + pcl::PointXYZ queryPt(0.1, 0, -0.3); + // 1st NN is at this index + int nnIdx = 0; + + // Return values from trilinear search + std::vector interpInds1, interpInds2; + std::vector interps1, interps2; + + // Search function. Search neighbors to do trilinear interpolation among + interp.FindTrilinearInterpolators(cloud, queryPt, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + + // Check that it finds 2 z slices + EXPECT_EQ(interpInds1.size(), 4); + EXPECT_EQ(interpInds2.size(), 4); + EXPECT_EQ(interps1.size(), 4); + EXPECT_EQ(interps2.size(), 4); + + // Check neighbors' indices. + // Degenerate into a 2D triangle. + // Top-down view of 4 nearest neighbors on 1st z slice, not in a rectangle, + // therefore the 8 neighbors together won't be in a prism: + // z = 0: + // y + // | + // 1 c d + // 0 -------a----b- x + // | + // -1 0 1 + // z = -2: + // y + // | + // 1 h f + // 0 --g----e------ x + // | + // -1 0 1 + EXPECT_EQ(interpInds1[0], 0); // a + EXPECT_EQ(interpInds1[1], 1); // b + EXPECT_EQ(interpInds1[2], 2); // c + EXPECT_EQ(interpInds1[3], 3); // d + + // 2nd z slice only has 4 points + EXPECT_EQ(interpInds2[0], 4); // e + EXPECT_EQ(interpInds2[1], 6); // f + EXPECT_EQ(interpInds2[2], 5); // g + EXPECT_EQ(interpInds2[3], 7); // h + + // Package into variable types for interpolation function + Eigen::Vector3f queryPtEigen(queryPt.x, queryPt.y, queryPt.z); + Eigen::MatrixXf interpsEigen(8, 3); + for (int i = 0; i < interpInds1.size(); ++i) + { + interpsEigen.row(i) = Eigen::Vector3f( + cloud.points[interpInds1[i]].x, + cloud.points[interpInds1[i]].y, + cloud.points[interpInds1[i]].z); + } + for (int i = 0; i < interpInds2.size(); ++i) + { + interpsEigen.row(4 + i) = Eigen::Vector3f( + cloud.points[interpInds2[i]].x, + cloud.points[interpInds2[i]].y, + cloud.points[interpInds2[i]].z); + } + + // Data to test interpolation between two z slices + std::vector data; + data.push_back( 0.0f); // a + data.push_back( 0.0f); // b + data.push_back( 0.0f); // c + data.push_back( 0.0f); // d + // Set values in 2nd z slice very different from those on 1st z slice. + // Since query point is entirely on 1st z slice, it should be independent + // of values on 2nd z slice. + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + + // Interpolation function + float result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Result should be between the two z slices + EXPECT_GT(result, 0); + EXPECT_LT(result, 1000); +} +*/ + +///////////////////////////////////////////////// +// Happens when trilinear interpolation assumption of points being corners of +// a prism is not satisfied, falls back to hybrid "barylinear" interpolation, +// i.e. linear interpolation between 3D non-prism quads in two z slices, or the +// degenerative cases of a 2D triangle, or a 1D line segment. +// This tests the 2D case from the 3D overloaded BarycentricInterpolate(). +TEST(InterpolationTest, TrilinearFallbackToHybridBarylinear2D) +{ + ignition::common::Console::SetVerbosity(4); + + Interpolation interp(TRILINEAR); + EXPECT_EQ(interp.Method(), TRILINEAR); + + interp.SetDebug(true); + interp.SetDebugMath(false); + + // Inputs to search function + float spatialRes = 0.1f; + int k = 4; + pcl::PointCloud cloud; + + // Populate input cloud + // Top-down view: + // y + // | + // 1 * * * + // q + // 0 --*----*----*- x + // | + // -1 0 1 + // z view: + // z + // 0| * * * + // -2| * * + // + // A rectangle at z = 0 + cloud.points.push_back(pcl::PointXYZ(-1, 0, 0)); + cloud.points.push_back(pcl::PointXYZ( 1, 0, 0)); + cloud.points.push_back(pcl::PointXYZ(-1, 1, 0)); + cloud.points.push_back(pcl::PointXYZ( 1, 1, 0)); + // Same rectangle at z = -2 + cloud.points.push_back(pcl::PointXYZ(-1, 0, -2)); + cloud.points.push_back(pcl::PointXYZ( 1, 0, -2)); + cloud.points.push_back(pcl::PointXYZ(-1, 1, -2)); + cloud.points.push_back(pcl::PointXYZ( 1, 1, -2)); + // A middle column at z = 0 only, between two sides of the rectangle + cloud.points.push_back(pcl::PointXYZ(0, 0, 0)); + cloud.points.push_back(pcl::PointXYZ(0, 1, 0)); + + // Query point q in middle column, slightly +x. Its nearest neighbors are not + // corners of a prism. + pcl::PointXYZ queryPt(0.1, 0.3, 0); + // 1st NN is at this index + int nnIdx = 8; + + // Return values from trilinear search + std::vector interpInds1, interpInds2; + std::vector interps1, interps2; + + // Search function. Search neighbors to do trilinear interpolation among + interp.FindTrilinearInterpolators(cloud, queryPt, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + + // Check that it finds 2 z slices + EXPECT_EQ(interpInds1.size(), 4); + EXPECT_EQ(interpInds2.size(), 4); + EXPECT_EQ(interps1.size(), 4); + EXPECT_EQ(interps2.size(), 4); + + // Check neighbors' indices. + // Degenerate into a 2D triangle. + // Top-down view of 4 nearest neighbors on 1st z slice, not in a rectangle, + // therefore the 8 neighbors together won't be in a prism: + // y + // | + // 1 c + // q + // 0 --d----a----b- x + // | + // -1 0 1 + EXPECT_EQ(interpInds1[0], 8); // a + EXPECT_EQ(interpInds1[1], 9); // b + EXPECT_EQ(interpInds1[2], 1); // c + EXPECT_EQ(interpInds1[3], 3); // d + + // 2nd z slice only has 4 points + EXPECT_EQ(interpInds2[0], 5); + EXPECT_EQ(interpInds2[1], 7); + EXPECT_EQ(interpInds2[2], 4); + EXPECT_EQ(interpInds2[3], 6); + + // Package into variable types for interpolation function + Eigen::Vector3f queryPtEigen(queryPt.x, queryPt.y, queryPt.z); + Eigen::MatrixXf interpsEigen(8, 3); + for (int i = 0; i < interpInds1.size(); ++i) + { + interpsEigen.row(i) = Eigen::Vector3f( + cloud.points[interpInds1[i]].x, + cloud.points[interpInds1[i]].y, + cloud.points[interpInds1[i]].z); + } + for (int i = 0; i < interpInds2.size(); ++i) + { + interpsEigen.row(4 + i) = Eigen::Vector3f( + cloud.points[interpInds2[i]].x, + cloud.points[interpInds2[i]].y, + cloud.points[interpInds2[i]].z); + } + + // Data to test interpolation between two z slices + std::vector data; + data.push_back( 0.0f); // a + data.push_back( 25.0f); // b + data.push_back( 50.0f); // c + data.push_back( 0.0f); // d + // Set values in 2nd z slice very different from those on 1st z slice. + // Since query point is entirely on 1st z slice, it should be independent + // of values on 2nd z slice. + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + + // Interpolation function + float result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Result should be among a, b, and c + EXPECT_GT(result, 0); + EXPECT_LT(result, 50); +} + +///////////////////////////////////////////////// +// Happens when trilinear interpolation assumption of points being corners of +// a prism is not satisfied, falls back to hybrid "barylinear" interpolation, +// i.e. linear interpolation between 3D non-prism quads in two z slices, or the +// degenerative cases of a 2D triangle, or a 1D line segment. +// This tests the 1D case from the 2D overloaded BarycentricInterpolate(). +TEST(InterpolationTest, TrilinearFallbackToHybridBarylinear1D) +{ + ignition::common::Console::SetVerbosity(4); + + Interpolation interp(TRILINEAR); + EXPECT_EQ(interp.Method(), TRILINEAR); + + interp.SetDebug(true); + interp.SetDebugMath(false); + + // Inputs to search function + float spatialRes = 0.1f; + int k = 4; + pcl::PointCloud cloud; + + // Populate input cloud + // Top-down view (each corner point is 2 points overlapped): + // y + // | + // 0 ---*-*-*q--*---- x + // | + // -1 0 1 + // z view: + // z + // 0| * * * * + // -2| * * * * + // + // A line at z = 0 + cloud.points.push_back(pcl::PointXYZ(-1, 0, 0)); + cloud.points.push_back(pcl::PointXYZ(-0.5, 0, 0)); + cloud.points.push_back(pcl::PointXYZ( 0, 0, 0)); + cloud.points.push_back(pcl::PointXYZ( 1, 0, 0)); + // Same line at z = -2 + cloud.points.push_back(pcl::PointXYZ(-1, 0, -2)); + cloud.points.push_back(pcl::PointXYZ(-0.5, 0, -2)); + cloud.points.push_back(pcl::PointXYZ( 0, 0, -2)); + cloud.points.push_back(pcl::PointXYZ( 1, 0, -2)); + + pcl::PointXYZ queryPt(0.1, 0, 0); + // 1st NN is at this index + int nnIdx = 2; + + // Return values from trilinear search + std::vector interpInds1, interpInds2; + std::vector interps1, interps2; + + // Search function. Search neighbors to do trilinear interpolation among + interp.FindTrilinearInterpolators(cloud, queryPt, nnIdx, spatialRes, + interpInds1, interps1, interpInds2, interps2, k); + + // Check that it finds 2 z slices + EXPECT_EQ(interpInds1.size(), 4); + EXPECT_EQ(interpInds2.size(), 4); + EXPECT_EQ(interps1.size(), 4); + EXPECT_EQ(interps2.size(), 4); + + // Check neighbors' indices. + // Degenerate into a 1D line. + // Top-down view: + // | + // 0 ---d-b-aq--c---- x + // | + // -1 0 1 + EXPECT_EQ(interpInds1[0], 2); // a + EXPECT_EQ(interpInds1[1], 1); // b + EXPECT_EQ(interpInds1[2], 3); // c + EXPECT_EQ(interpInds1[3], 0); // d + + // 2nd z slice only has 4 points + EXPECT_EQ(interpInds2[0], 6); + EXPECT_EQ(interpInds2[1], 5); + EXPECT_EQ(interpInds2[2], 7); + EXPECT_EQ(interpInds2[3], 4); + + // Package into variable types for interpolation function + Eigen::Vector3f queryPtEigen(queryPt.x, queryPt.y, queryPt.z); + Eigen::MatrixXf interpsEigen(8, 3); + for (int i = 0; i < interpInds1.size(); ++i) + { + interpsEigen.row(i) = Eigen::Vector3f( + cloud.points[interpInds1[i]].x, + cloud.points[interpInds1[i]].y, + cloud.points[interpInds1[i]].z); + } + for (int i = 0; i < interpInds2.size(); ++i) + { + interpsEigen.row(4 + i) = Eigen::Vector3f( + cloud.points[interpInds2[i]].x, + cloud.points[interpInds2[i]].y, + cloud.points[interpInds2[i]].z); + } + + // Data to test interpolation between two z slices + std::vector data; + data.push_back( 50.0f); // a + data.push_back( 25.0f); // b + data.push_back( 100.0f); // c + data.push_back( 0.0f); // d + // Set values in 2nd z slice very different from those on 1st z slice. + // Since query point is entirely on 1st z slice, it should be independent + // of values on 2nd z slice. + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + data.push_back(1000.0f); + + // Interpolation function + float result = interp.InterpolateData(queryPtEigen, interpsEigen, data); + // Result should be between closest points on the line to q + EXPECT_GT(result, 50); + EXPECT_LT(result, 100); +} +} diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 882ebd8e..324a47e2 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -33,12 +33,11 @@ #include #include -#include -#include #include #include #include +#include "Interpolation.hh" #include "ScienceSensorsSystem.hh" using namespace tethys; @@ -58,136 +57,14 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Generate octree from spatial data, for searching public: void GenerateOctrees(); - /// \brief Interpolate among existing science data to output an estimated - /// reading at the current sensor location. - /// \param[in] _p Position to interpolate for - /// \param[in] _xyzs XYZ coordinates of existing data locations - /// \param[in] _values Data values at the locations - /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float InterpolateData( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values); - - /// \brief True to use trilinear interpolation, false to use barycentric - /// interpolation - public: bool useTrilinear = true; - - /// \brief Convert a vector of PCL points to an Eigen::Matrix. + /// \brief Helper. Convert a vector of PCL points to an Eigen::Matrix. /// \param[in] _vec Source vector - /// \param[out] _mat Result matrix + /// \param[out] _mat Result matrix. One point per row. public: void PclVectorToEigen( const std::vector &_vec, Eigen::MatrixXf &_mat); - ////////////////////////////////// - // Trilinear interpolation - - /// \brief Comparison function for std::set_difference(). - /// Comparison between points is arbitrary. This function is only used for - /// set operations, not for literal sorting. - public: bool comparePclPoints( - const pcl::PointXYZ &a, - const pcl::PointXYZ &b); - - /// \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] _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, - pcl::PointCloud &_cloud, - pcl::PointCloud &_zSlice, - std::vector &_zSliceInds, - bool _invert=false); - - /// \brief Create an octree from a point cloud, and search for _k nearest - /// neighbors. - /// \param[in] _searchPt Location in space to search from - /// \param[in] _cloud Point cloud to search in - /// \param[out] _nbrInds Result of octree search, indices of points. - /// \param[out] _nbrSqrDists Result of octree search, distances. - /// \param[out] _nbrs XYZ of the k nearest neighbors found. - /// \param[in] _k Number of nearest neighbors. Default to 4, for trilinear - /// interpolation between two z slices of 4 points per slice. - public: void CreateAndSearchOctree( - pcl::PointXYZ &_searchPt, - pcl::PointCloud &_cloud, - std::vector &_nbrInds, - std::vector &_nbrSqrDists, - std::vector &_nbrs, - int _k=4); - - /// \brief Trilinear interpolation for a point inside a prism, given 8 - /// verticies of the prism. Suitable for data laid out in a rectangular grid. - /// Otherwise, use a different interpolation method, e.g. barycentric. - /// \param[in] _p Position to interpolate for - /// \param[in] _xyzs XYZ coordinates of 8 vertices of a prism - /// \param[in] _values Data values at the 8 vertices - /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float TrilinearInterpolate( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values); - - ////////////////////////////////// - // Barycentric interpolation - - /// \brief Barycentric interpolation in 3D, given 4 points on any arbitrary - /// tetrahedra. - /// \param[in] _p Position within the tetrahedra to interpolate for - /// \param[in] _xyzs n x 3. XYZ coordinates of 4 vertices of a tetrahedra - /// \param[in] _values Data values at the 4 vertices - /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float BarycentricInterpolate( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values); - - /// \brief Barycentric interpolation in 2D, given 4 points on a plane. Finds - /// 3 points on a triangle within which the query point lies, then - /// interpolates using them. - /// \param[in] _p Position within the triangle to interpolate for - /// \param[in] _xys n x 2. XY coordinates of 3 vertices of a triangle - /// \param[in] _values Data values at the 3 vertices - /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float BarycentricInterpolate( - const Eigen::Vector2f &_p, - const Eigen::Matrix &_xys, - const std::vector &_values); - - /// \brief 1D linear interpolation, given 4 points on a line. Finds 2 points - /// on a segment within which the query point lies, then interpolates using - /// them. - /// \param[in] _p Position to interpolate for - /// \param[in] _xs Positions to interpolate from - /// \param[in] _values Data values at the positions to interpolate from - /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float BarycentricInterpolate( - const float &_p, - const Eigen::VectorXf &_xs, - const std::vector &_values); - - /// \brief Extract elements at indices _inds from _orig vector. + /// \brief Helper. Extract elements at indices _inds from _orig vector. /// \param[in] _orig A vector of values to extract /// \param[in] _inds Indices of elements to extract /// \param[out] _new Extracted values @@ -196,15 +73,6 @@ class tethys::ScienceSensorsSystemPrivate const std::vector &_inds, std::vector &_new); - /// \brief Sort vector, store indices to original vector after sorting. - /// Original vector unchanged. - /// \param[in] _v Vector to sort - /// \param[out] _idx Indices of original vector in sorted vector - public: template - void SortIndices( - const std::vector &_v, - std::vector &_idx); - ////////////////////////////// // Functions for communication @@ -286,10 +154,6 @@ class tethys::ScienceSensorsSystemPrivate /// TODO: Compute resolution from data. See where this constant is used. public: const float INTERPOLATE_DIST_THRESH = 5.0; - /// \brief Debug printouts for interpolation. Will keep around at least until - /// interpolation is stable. - public: const bool DEBUG_INTERPOLATE = false; - /////////////////////////////// // Variables for coordinate system @@ -352,6 +216,9 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Science data. Same size as temperatureArr. public: std::vector> northCurrentArr; + /// \brief Object for data interpolation + public: Interpolation interpolation; + ////////////////////////////// // Variables for communication @@ -684,22 +551,6 @@ void ScienceSensorsSystemPrivate::GenerateOctrees() } } -///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::InterpolateData( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values) -{ - if (this->useTrilinear) - { - return this->TrilinearInterpolate(_p, _xyzs, _values); - } - else - { - return this->BarycentricInterpolate(_p, _xyzs, _values); - } -} - ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::PclVectorToEigen( const std::vector &_vec, @@ -714,601 +565,6 @@ void ScienceSensorsSystemPrivate::PclVectorToEigen( } } -///////////////////////////////////////////////// -bool ScienceSensorsSystemPrivate::comparePclPoints( - const pcl::PointXYZ &a, - const pcl::PointXYZ &b) -{ - // Comparison between points is arbitrary. This function is only used for - // set operations, not for literal sorting. - return a.x < b.x && a.y < b.y && a.z < b.z; -} - -///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::FindTrilinearInterpolators( - pcl::PointXYZ &_pt, - std::vector &_inds, - std::vector &_sqrDists, - std::vector &_interpolators1, - std::vector &_interpolators2) -{ - // Initialize return parameters - _interpolators1.clear(); - _interpolators2.clear(); - - // Sanity checks - // Vector not populated - if (this->timeSpaceCoords.size() == 0) - { - return; - } - // Point cloud not populated - if (this->timeSpaceCoords[this->timeIdx]->size() == 0) - { - 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; - // Indices of points in the z slices - std::vector zSliceInds1, zSliceInds2; - - // Indices and distances of neighboring points in the search results - // 4 above, 4 below - 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, - zSliceInds1); - igndbg << "1st nn idx " << nnIdx << ", dist " << sqrt(minDist) - << ", 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 - << " for trilinear interpolation." << std::endl; - return; - } - - // Step 2: exclude z slice of 1st NN from further searches. - - pcl::PointCloud cloudExceptZSlice1; - std::vector indsExceptZSlice1; - - // Convert input indices to PCL type - pcl::PointIndices::Ptr zSliceInds1pcl(new pcl::PointIndices()); - zSliceInds1pcl->indices = zSliceInds1; - - // Remove all points in the z slice of the 1st NN, so that the 2nd NN will be - // found in another z slice. - pcl::ExtractIndices extract; - extract.setInputCloud(this->timeSpaceCoords[this->timeIdx]->makeShared()); - extract.setIndices(zSliceInds1pcl); - extract.setNegative(false); - extract.filter(cloudExceptZSlice1); - extract.filter(indsExceptZSlice1); - - 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; - 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 - << " for trilinear interpolation." << std::endl; - return; - } -} - -///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::CreateDepthSlice( - float _depth, - pcl::PointCloud &_cloud, - pcl::PointCloud &_zSlice, - std::vector &_zSliceInds, - bool _invert) -{ - // Separate a z slice, i.e. points with z equal to that of 1st NN - // Pass in _invert=true to get indices of removed points - pcl::PassThrough passThruFilter = - pcl::PassThrough(true); - passThruFilter.setInputCloud(_cloud.makeShared()); - passThruFilter.setNegative(_invert); - passThruFilter.setFilterFieldName("z"); - passThruFilter.setFilterLimits(_depth - 1e-6, _depth + 1e-6); - passThruFilter.filter(_zSlice); - - // Get indices of points kept. Default method returns removed indices, so - // invert the filter to get indices of points kept. - passThruFilter.setNegative(!_invert); - passThruFilter.filter(_zSliceInds); -} - -///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::CreateAndSearchOctree( - pcl::PointXYZ &_searchPt, - pcl::PointCloud &_cloud, - std::vector &_nbrInds, - std::vector &_nbrSqrDists, - std::vector &_nbrs, - int _k) -{ - // Initialize return value populated by hand - _nbrs.clear(); - - // Create octree for cloud - auto octree = pcl::octree::OctreePointCloudSearch( - this->spatialRes); - octree.setInputCloud(_cloud.makeShared()); - octree.addPointsFromInputCloud(); - - // Search in the depth slice to find 4 closest neighbors - if (octree.getLeafCount() > 0) - { - if (octree.nearestKSearch( - _searchPt, _k, _nbrInds, _nbrSqrDists) <= 0) - { - ignwarn << "No data found near search point " << _searchPt - << std::endl; - return; - } - else - { - igndbg << "Found " << _nbrInds.size() << " neighbors." - << std::endl; - - for (std::size_t i = 0; i < _nbrInds.size(); ++i) - { - pcl::PointXYZ nbrPt = _cloud.at(_nbrInds[i]); - _nbrs.push_back(nbrPt); - - igndbg << "Neighbor at (" - << nbrPt.x << ", " << nbrPt.y << ", " << nbrPt.z << "), " - << "distance " << sqrt(_nbrSqrDists[i]) << " m" << std::endl; - } - } - } - else - { - ignwarn << "Zero points in this octree." << std::endl; - } -} - - -///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::TrilinearInterpolate( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values) -{ - IGN_PROFILE("ScienceSensorsSystemPrivate::TrilinearInterpolate"); - - // Sanity check: Must have 8 points, 4 above, 4 below. - if (_xyzs.rows() != 8) - { - ignerr << "Size of interpolators invalid (" << _xyzs.size() << "). " - << "Need 8 points in a rectangular prism. " - << "Cannot perform trilinear interpolation." << std::endl; - return std::numeric_limits::quiet_NaN(); - } - - // Find closest neighbor - Eigen::MatrixXf::Index minR, minC; - - // 3 x n - Eigen::MatrixXf diff = _xyzs.transpose().colwise() - _p; - igndbg << "diff: " << std::endl << diff << std::endl; - - Eigen::VectorXf dists = diff.colwise().norm(); - igndbg << "dists: " << std::endl << dists << std::endl; - float minDist = dists.minCoeff(&minC); - igndbg << "minI: " << minC << std::endl; - - // Dummy: Just return the closest element - return _values[minC]; - - - // TODO trilinear interpolation using the 8 interpolators found -} - -///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::BarycentricInterpolate( - const Eigen::Vector3f &_p, - const Eigen::MatrixXf &_xyzs, - const std::vector &_values) -{ - // Implemented from https://en.wikipedia.org/wiki/Barycentric_coordinate_system#Barycentric_coordinates_on_tetrahedra - - if (this->DEBUG_INTERPOLATE) - igndbg << "_p: " << std::endl << _p << std::endl; - - Eigen::Matrix3f T; - // Row 1 is x-coords: x1 - x4, x2 - x4, x3 - x4 - T << _xyzs(0, 0) - _xyzs(3, 0), - _xyzs(1, 0) - _xyzs(3, 0), - _xyzs(2, 0) - _xyzs(3, 0), - // Row 2 is y-coords: y1 - y4, y2 - y4, y3 - y4 - _xyzs(0, 1) - _xyzs(3, 1), - _xyzs(1, 1) - _xyzs(3, 1), - _xyzs(2, 1) - _xyzs(3, 1), - // Row 3 is z-coords: z1 - z4, z2 - z4, z3 - z4 - _xyzs(0, 2) - _xyzs(3, 2), - _xyzs(1, 2) - _xyzs(3, 2), - _xyzs(2, 2) - _xyzs(3, 2); - if (this->DEBUG_INTERPOLATE) - igndbg << "T: " << std::endl << T << std::endl; - - int zeroRowCount = 0; - bool rowIsZero [3] = {false, false, false}; - for (int r = 0; r < T.rows(); ++r) - { - if ((T.row(r).array().abs() < 1e-6).all()) - { - zeroRowCount++; - rowIsZero[r] = true; - } - } - - // If exactly 1 row of T is all zeros, then the points are in a 2D plane. - // 2D. Interpolate on a plane. Otherwise T inverse will result in nans. - if (zeroRowCount == 1) - { - if (this->DEBUG_INTERPOLATE) - igndbg << "4 points are on a plane. Using 2D barycentric interpolation " - "for a triangle." << std::endl; - - // Eliminate the constant axis - Eigen::Vector2f p2D; - Eigen::Matrix xyzs2D; - int nextCol = 0; - for (int r = 0; r < T.rows(); ++r) - { - if (!rowIsZero[r]) - { - // Populate the axes corresponding to nonzero rows of T. - // E.g. If row 1 of T is zeros, then points are on x-plane. Ignore - // x-coordinates, which are on column 1 of the original points matrix. - p2D(nextCol) = _p(r); - xyzs2D.col(nextCol) = _xyzs.col(r); - ++nextCol; - } - } - return this->BarycentricInterpolate(p2D, xyzs2D, _values); - } - // 1D. Interpolate on a line. Otherwise T inverse will result in nans. - else if (zeroRowCount == 2) - { - if (this->DEBUG_INTERPOLATE) - igndbg << "4 points are on a line. Using 1D interpolation." << std::endl; - - float p1D; - Eigen::VectorXf xyzs1D(_xyzs.rows()); - for (int r = 0; r < T.rows(); ++r) - { - // Only one row is non-zero - if (!rowIsZero[r]) - { - p1D = _p(r); - xyzs1D = _xyzs.col(r); - } - } - return this->BarycentricInterpolate(p1D, xyzs1D, _values); - } - // T is entirely zero. Then all points are at the same point. Take any value. - else if (zeroRowCount == 3) - { - if (this->DEBUG_INTERPOLATE) - igndbg << "4 points are at the exact same point. Arbitrarily selecting " - "one of their values as interpolation result." << std::endl; - return _values[0]; - } - - // r4 = (x4, y4, z4) - Eigen::Vector3f r4; - r4 << _xyzs(3, 0), _xyzs(3, 1), _xyzs(3, 2); - if (this->DEBUG_INTERPOLATE) - igndbg << "r4: " << std::endl << r4 << std::endl; - - // (lambda1, lambda2, lambda3) - Eigen::Vector3f lambda123 = T.inverse() * (_p - r4); - - if (this->DEBUG_INTERPOLATE) - igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; - - // lambda4 = 1 - lambda1 - lambda2 - lambda3 - float lambda4 = 1 - lambda123(0) - lambda123(1) - lambda123(2); - - if (this->DEBUG_INTERPOLATE) - igndbg << "Barycentric 3D lambda 1 2 3 4: " << lambda123(0) << ", " - << lambda123(1) << ", " - << lambda123(2) << ", " - << lambda4 << std::endl; - - // f(r) = lambda1 * f(r1) + lambda2 * f(r2) + lambda3 * f(r3) - float result = - lambda123(0) * _values[0] + - lambda123(1) * _values[1] + - lambda123(2) * _values[2] + - lambda4 * _values[3]; - - if (this->DEBUG_INTERPOLATE) - igndbg << "Barycentric 3D interpolation of values " << _values[0] << ", " - << _values[1] << ", " << _values[2] << ", " << _values[3] - << " resulted in " << result << std::endl; - - return result; -} - -///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::BarycentricInterpolate( - const Eigen::Vector2f &_p, - const Eigen::Matrix &_xys, - const std::vector &_values) -{ - if (this->DEBUG_INTERPOLATE) - { - igndbg << "_p: " << std::endl << _p << std::endl; - igndbg << "_xys: " << std::endl << _xys << std::endl; - } - - // 2D case, consider inputs a triangle and use 2 x 2 matrix for T - Eigen::Matrix2f T(2, 2); - Eigen::Vector2f lastVert; - Eigen::Vector2f lambda12; - float lambda3; - - // Eliminate the correct point, so that we have a triangle that the query - // point lies within. - for (int r = 0; r < _xys.rows(); ++r) - { - Eigen::Matrix xys3; - int nextRow = 0; - // Populate temp matrix with all points except current point (row) - for (int r2 = 0; r2 < xys3.rows(); ++r2) - { - if (r2 == r) - { - continue; - } - xys3.row(nextRow++) = _xys.row(r2); - } - if (this->DEBUG_INTERPOLATE) - igndbg << "xys3: " << std::endl << xys3 << std::endl; - - // Row 1: x1 - x3, x2 - x3 - T << xys3(0, 0) - xys3(2, 0), - xys3(1, 0) - xys3(2, 0), - // Row 2: y1 - y3, y2 - y3 - xys3(0, 1) - xys3(2, 1), - xys3(1, 1) - xys3(2, 1); - if (this->DEBUG_INTERPOLATE) - igndbg << "T: " << std::endl << T << std::endl; - - // lastVert = (x3, y3) - lastVert << xys3(2, 0), xys3(2, 1); - if (this->DEBUG_INTERPOLATE) - igndbg << "lastVert: " << std::endl << lastVert << std::endl; - - // (lambda1, lambda2) - lambda12 = T.inverse() * (_p - lastVert); - - if (this->DEBUG_INTERPOLATE) - igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; - - // lambda3 = 1 - lambda1 - lambda2 - lambda3 = 1 - lambda12(0) - lambda12(1); - - if (this->DEBUG_INTERPOLATE) - igndbg << "Barycentric 2D lambda 1 2 3: " << lambda12(0) << ", " - << lambda12(1) << ", " - << lambda3 << std::endl; - - // If all lambdas >= 0, then we found a triangle that the query point - // lies within. (A lambda would be negative if point is outside triangle) - if ((lambda12.array() >= 0).all() && lambda3 >= 0) - { - break; - } - } - - // f(r) = lambda1 * f(r1) + lambda2 * f(r2) - float result = - lambda12(0) * _values[0] + - lambda12(1) * _values[1] + - lambda3 * _values[2]; - - if (this->DEBUG_INTERPOLATE) - igndbg << "Barycentric 2D interpolation of values " << _values[0] << ", " - << _values[1] << ", " << _values[2] - << " resulted in " << result << std::endl; - - return result; -} - -///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::BarycentricInterpolate( - const float &_p, - const Eigen::VectorXf &_xs, - const std::vector &_values) -{ - if (this->DEBUG_INTERPOLATE) - { - igndbg << "_p: " << std::endl << _p << std::endl; - igndbg << "_xs: " << std::endl << _xs << std::endl; - } - - // If _p is equal to one of the points, just take the value of that point. - // This is to catch floating point errors if _p lies on one side of all - // points in _xs, but really equal to one of the endpoints. - if (((_xs.array() - _p).abs() < 1e-6).any()) - { - for (int i = 0; i < _xs.size(); ++i) - { - if (abs(_xs(i) - _p) < 1e-6) - { - if (this->DEBUG_INTERPOLATE) - igndbg << "_p lies on a neighbor. " - << "1D linear interpolation of values " << _values[0] << ", " - << _values[1] << ", " << _values[2] << ", " << _values[3] - << " resulted in " << _values[i] << std::endl; - return _values[i]; - } - } - } - - // If _p lies on one side of all points in _xs, then cannot interpolate. - if ((_xs.array() - _p < 0).all() || - (_xs.array() - _p > 0).all()) - { - ignwarn << "1D linear interpolation: query point lies on one side of all " - "interpolation points. Cannot interpolate." << std::endl; - return std::numeric_limits::quiet_NaN(); - } - - // Sort points and store the indices - std::vector xsSorted; - std::vector 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{-1}; - int gtPSortedIdx{-1}; - float ltPDist, gtPDist; - - // Get the two closest positions in _xs that _p lies between. - for (int i = 0; i < xsSorted.size() - 1; ++i) - { - // Two consecutive elements in the sorted vector, that the query point lies - // between, are the closest points to each side of the query point. - if (xsSorted[i] <= _p && _p <= xsSorted[i+1]) - { - ltPSortedIdx = i; - gtPSortedIdx = i + 1; - - ltPDist = _p - xsSorted[i]; - gtPDist = xsSorted[i+1] - _p; - - break; - } - } - - // 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::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::quiet_NaN(); - } - float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx]; - - if (this->DEBUG_INTERPOLATE) - { - igndbg << "ltPWeight: " << ltPWeight << ", gtPWeight: " << gtPWeight - << std::endl; - igndbg << "1D linear interpolation of values " << _values[0] << ", " - << _values[1] << ", " << _values[2] << ", " << _values[3] - << " resulted in " << result << std::endl; - } - - return result; -} - ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::ExtractElements( const std::vector &_orig, @@ -1323,25 +579,6 @@ void ScienceSensorsSystemPrivate::ExtractElements( } } -///////////////////////////////////////////////// -template -void ScienceSensorsSystemPrivate::SortIndices( - const std::vector &_v, - std::vector &_idx) -{ - // From https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes - - // Initialize original index locations - _idx.resize(_v.size()); - std::iota(_idx.begin(), _idx.end(), 0); - - // 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 - std::stable_sort(_idx.begin(), _idx.end(), - [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];}); -} - ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::PublishData() { @@ -1383,6 +620,8 @@ void ScienceSensorsSystem::Configure( ignmsg << "Loading science data from [" << this->dataPtr->dataPath << "]" << std::endl; } + + this->dataPtr->interpolation.SetMethod(TRILINEAR); } ///////////////////////////////////////////////// @@ -1506,18 +745,24 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // Barycentric interpolation searches 4 neighbors directly int initK = 4; - // Trilinear interpolation starts by searching for 1 neighbor - if (this->dataPtr->useTrilinear) + // Trilinear interpolation starts by searching for 1 neighbor. Only its z is + // used, to find the nearest z slice. + if (this->dataPtr->interpolation.Method() == TRILINEAR) { initK = 1; } + // Number of points per z slice, for trilinear interpolation + int nbrsPerZSlice = 4; + // Indices and distances of neighboring points to sensor position - std::vector spatialIdx; - std::vector spatialSqrDist; + std::vector spatialInds; + std::vector spatialSqrDists; - // Positions of neighbors to use in interpolation + // Positions of neighbors to use in interpolation, and their indices in + // original point cloud std::vector interpolatorXYZs; + std::vector interpolatorInds; // Get a sensor's position, search in the octree for the closest neighbors. // Only need to done for one sensor. All sensors are on the robot, doesn't @@ -1544,7 +789,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, break; } - if (this->dataPtr->DEBUG_INTERPOLATE) + if (this->dataPtr->interpolation.Debug()) { igndbg << "Searching around sensor Cartesian location " << std::round(searchPoint.x * 1000.0) / 1000.0 << ", " @@ -1555,86 +800,111 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // If there are any nodes in the octree, search in octree to find spatial // index of science data if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx]->getLeafCount() - > 0) + == 0) + { + break; + } + + IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch"); + + // kNN search (alternatives are voxel search and radius search. kNN + // search is good for variable resolution when the distance to the next + // neighbor is unknown). + // Search for nearest neighbors + if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx] + ->nearestKSearch(searchPoint, initK, spatialInds, spatialSqrDists) <= 0) + { + ignwarn << "Not enough data found near sensor location " << sensorPosENU + << std::endl; + continue; + } + // Debug output + else if (this->dataPtr->interpolation.Debug()) { - IGN_PROFILE("ScienceSensorsSystem::PostUpdate nearestKSearch"); - - // kNN search (alternatives are voxel search and radius search. kNN - // search is good for variable resolution when the distance to the next - // neighbor is unknown). - // Search for nearest neighbors - if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx] - ->nearestKSearch(searchPoint, initK, spatialIdx, spatialSqrDist) <= 0) + for (std::size_t i = 0; i < spatialInds.size(); i++) + { + // Index the point cloud at the current time slice + pcl::PointXYZ nbrPt = this->dataPtr->timeSpaceCoords[ + this->dataPtr->timeIdx]->at(spatialInds[i]); + + igndbg << "Neighbor at (" + << std::round(nbrPt.x * 1000) / 1000.0 << ", " + << std::round(nbrPt.y * 1000) / 1000.0 << ", " + << std::round(nbrPt.z * 1000) / 1000.0 + << "), squared distance " << spatialSqrDists[i] + << " m" << std::endl; + } + } + reinterpolate = true; + + if (this->dataPtr->interpolation.Method() == TRILINEAR) + { + // Vector not populated + if (this->dataPtr->timeSpaceCoords.size() == 0) { - ignwarn << "Not enough data found near sensor location " << sensorPosENU - << std::endl; continue; } - // Debug output - else if (this->dataPtr->DEBUG_INTERPOLATE) + // No nearest neighbors found, probably meaning cloud has only 1 point + if (spatialInds.size() < 1) { - for (std::size_t i = 0; i < spatialIdx.size(); i++) - { - // Index the point cloud at the current time slice - pcl::PointXYZ nbrPt = this->dataPtr->timeSpaceCoords[ - this->dataPtr->timeIdx]->at(spatialIdx[i]); - - igndbg << "Neighbor at (" - << std::round(nbrPt.x * 1000) / 1000.0 << ", " - << std::round(nbrPt.y * 1000) / 1000.0 << ", " - << std::round(nbrPt.z * 1000) / 1000.0 - << "), squared distance " << spatialSqrDist[i] - << " m" << std::endl; - } + continue; } - reinterpolate = true; - // TODO: refactor so that this if-stmt is replaced with one call. Use the - // Boolean in that new function. - if (this->dataPtr->useTrilinear) + // 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; + std::vector interpolatorInds1, interpolatorInds2; + this->dataPtr->interpolation.FindTrilinearInterpolators( + *(this->dataPtr->timeSpaceCoords[this->dataPtr->timeIdx]), + searchPoint, spatialInds[0], this->dataPtr->spatialRes, + interpolatorInds1, interpolatorsSlice1, + interpolatorInds2, interpolatorsSlice2, nbrsPerZSlice); + + if (interpolatorsSlice1.size() < nbrsPerZSlice || + interpolatorsSlice2.size() < nbrsPerZSlice) { - // 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); - if (interpolatorsSlice1.size() < 4 || interpolatorsSlice2.size() < 4) - { - ignwarn << "Could not find trilinear interpolators near sensor location " - << sensorPosENU << std::endl; - continue; - } + ignwarn << "Could not find trilinear interpolators near sensor " + << "location " << sensorPosENU << std::endl; + continue; + } + + // Concatenate the 2 sets of 4 points into a vector of 8 points + interpolatorXYZs.reserve(interpolatorsSlice1.size() + + interpolatorsSlice2.size()); + interpolatorXYZs.insert(interpolatorXYZs.end(), + interpolatorsSlice1.begin(), interpolatorsSlice1.end()); + interpolatorXYZs.insert(interpolatorXYZs.end(), + interpolatorsSlice2.begin(), interpolatorsSlice2.end()); - // Concatenate the 2 sets of 4 points into a vector of 8 points - interpolatorXYZs.reserve(interpolatorsSlice1.size() + - interpolatorsSlice2.size()); - interpolatorXYZs.insert(interpolatorXYZs.end(), - interpolatorsSlice1.begin(), interpolatorsSlice1.end()); - interpolatorXYZs.insert(interpolatorXYZs.end(), - interpolatorsSlice2.begin(), interpolatorsSlice2.end()); - - // FIXME in FindTrilinearInterpolators(): - // When call TrilinearInterpolate(): - // Find 1D indices of the data values d000-d111!!! Manually keep track - // of indices of the 4 points in the 2nd z slice (interpolatorsSlice2), - // which are scrambled up after the 1st z slice is removed from cloud! - - // FIXME: 4 neighbors found are not on a rectangle! That voids assumption - // of trilinear interpolation. Cannot perform valid interpolation. + // Prepare neighbor data to pass to interpolation + for (int i = 0; i < interpolatorInds1.size(); ++i) + { + interpolatorInds.push_back(interpolatorInds1[i]); } - else + for (int i = 0; i < interpolatorInds2.size(); ++i) { - // Get neighbor XYZs to pass to interpolation - for (int i = 0; i < spatialIdx.size(); ++i) - { - interpolatorXYZs.push_back(this->dataPtr->timeSpaceCoords[ - this->dataPtr->timeIdx]->at(spatialIdx[i])); - } + interpolatorInds.push_back(interpolatorInds2[i]); + } + } + else if (this->dataPtr->interpolation.Method() == BARYCENTRIC) + { + // Prepare neighbor data to pass to interpolation + for (int i = 0; i < spatialInds.size(); ++i) + { + interpolatorInds.push_back(spatialInds[i]); + interpolatorXYZs.push_back(this->dataPtr->timeSpaceCoords[ + this->dataPtr->timeIdx]->at(spatialInds[i])); } - - // Update last update position to the current position - this->dataPtr->lastSensorPosENU = sensorPosENU; } + else + { + ignerr << "INTERPOLATION_METHOD value invalid. " + << "Choose a valid interpolation method." << std::endl; + break; + } + + // Update last update position to the current position + this->dataPtr->lastSensorPosENU = sensorPosENU; // Only need to find position ONCE for the entire robot. Don't need to // repeat for every sensor. @@ -1660,24 +930,24 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // For the correct sensor, interpolate using nearby locations with data if (auto casted = std::dynamic_pointer_cast(sensor)) { - if (this->dataPtr->DEBUG_INTERPOLATE) + if (this->dataPtr->interpolation.Debug()) igndbg << "Interpolating salinity" << std::endl; this->dataPtr->ExtractElements( - this->dataPtr->salinityArr[this->dataPtr->timeIdx], spatialIdx, - interpolationValues); - float sal = this->dataPtr->InterpolateData( + this->dataPtr->salinityArr[this->dataPtr->timeIdx], + interpolatorInds, interpolationValues); + float sal = this->dataPtr->interpolation.InterpolateData( sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); casted->SetData(sal); } else if (auto casted = std::dynamic_pointer_cast( sensor)) { - if (this->dataPtr->DEBUG_INTERPOLATE) + if (this->dataPtr->interpolation.Debug()) igndbg << "Interpolating temperature" << std::endl; this->dataPtr->ExtractElements( - this->dataPtr->temperatureArr[this->dataPtr->timeIdx], spatialIdx, - interpolationValues); - float temp = this->dataPtr->InterpolateData( + this->dataPtr->temperatureArr[this->dataPtr->timeIdx], + interpolatorInds, interpolationValues); + float temp = this->dataPtr->interpolation.InterpolateData( sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); ignition::math::Temperature tempC; @@ -1687,33 +957,33 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, else if (auto casted = std::dynamic_pointer_cast( sensor)) { - if (this->dataPtr->DEBUG_INTERPOLATE) + if (this->dataPtr->interpolation.Debug()) igndbg << "Interpolating chlorophyll" << std::endl; this->dataPtr->ExtractElements( - this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx], spatialIdx, - interpolationValues); - float chlor = this->dataPtr->InterpolateData( + this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx], + interpolatorInds, interpolationValues); + float chlor = this->dataPtr->interpolation.InterpolateData( sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); casted->SetData(chlor); } else if (auto casted = std::dynamic_pointer_cast( sensor)) { - if (this->dataPtr->DEBUG_INTERPOLATE) + if (this->dataPtr->interpolation.Debug()) igndbg << "Interpolating E and N currents" << std::endl; this->dataPtr->ExtractElements( - this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx], spatialIdx, - interpolationValues); - float eCurr = this->dataPtr->InterpolateData( + this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx], + interpolatorInds, interpolationValues); + float eCurr = this->dataPtr->interpolation.InterpolateData( sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); // Reset before reuse interpolationValues.clear(); this->dataPtr->ExtractElements( - this->dataPtr->northCurrentArr[this->dataPtr->timeIdx], spatialIdx, - interpolationValues); - float nCurr = this->dataPtr->InterpolateData( + this->dataPtr->northCurrentArr[this->dataPtr->timeIdx], + interpolatorInds, interpolationValues); + float nCurr = this->dataPtr->interpolation.InterpolateData( sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); auto curr = ignition::math::Vector3d(eCurr, nCurr, 0.0); diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index f7d18044..88931281 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -73,7 +73,7 @@ - + @@ -89,6 +89,8 @@ 2003080103_mb_l3_las.csv + +