tools.h

Go to the documentation of this file.

#pragma once
 
#include <set>
 
#include <Eigen/Geometry>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PointIndices.h>
 
#include <pcl/point_types_conversion.h>
#include <pcl/common/io.h>
 
 
namespace Eigen
{
    using Hyperplane3f = Hyperplane<float, 3>;
}
 
 
namespace visionx
{
 
    Eigen::Vector3f pointCloudMedian(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                                     const pcl::PointIndices& indices = {});
 
 
    Eigen::Hyperplane3f fitPlaneSVD(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                                    const pcl::PointIndices& indices,
                                    Eigen::Vector3f center = Eigen::Vector3f::Zero());
    Eigen::Hyperplane3f fitPlaneSVD(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                                    Eigen::Vector3f center = Eigen::Vector3f::Zero());
 
    /// Flip `plane` if its normal does not point towards `normal`.
    Eigen::Hyperplane3f alignPlaneNormal(Eigen::Hyperplane3f plane, Eigen::Vector3f normal);
 
 
    pcl::PointIndicesPtr getPlaneInliers(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                                         Eigen::Hyperplane3f plane, float maxDistance);
 
 
    float hsvDistance(const pcl::PointXYZHSV& lhs, const pcl::PointXYZHSV& rhs,
                      const Eigen::Vector3f& hsvWeights = Eigen::Vector3f::Ones());
    float hsvDistance(const Eigen::Vector3f& lhs, const Eigen::Vector3f& rhs,
                      const Eigen::Vector3f& hsvWeights = Eigen::Vector3f::Ones());
 
 
    Eigen::Vector3f hsvMean(const pcl::PointCloud<pcl::PointXYZHSV>& hsvCloud, const std::vector<int>& indices);
 
    template <class PointRGB>
    Eigen::Vector3f hsvMean(const pcl::PointCloud<PointRGB>& rgbCloud, const std::vector<int>& indices)
    {
        pcl::PointCloud<pcl::PointXYZHSV> hsvCloud;
        pcl::PointCloudXYZRGBAtoXYZHSV(rgbCloud, hsvCloud);
        return hsvMean(hsvCloud, indices);
    }
 
 
    template <class PointT>
    void copyPointCloudWithoutIndices(const pcl::PointCloud<PointT>& input, const std::vector<int>& indices,
                                      pcl::PointCloud<PointT>& output)
    {
        const std::set<int> indexSet(indices.begin(), indices.end());
        output.clear();
        output.reserve(input.size() - indexSet.size());
        for (size_t i = 0; i < input.size(); ++i)
        {
            if (indexSet.count(int(i)) == 0)
            {
                output.push_back(input[i]);
            }
        }
    }
 
 
    template <class PointT>
    typename pcl::PointCloud<PointT>::Ptr getPointCloudWithoutIndices(
        const pcl::PointCloud<PointT>& input, const std::vector<int>& indices)
    {
        typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
        copyPointCloudWithoutIndices(input, indices, *output);
        return output;
    }
 
 
    template <class PointT>
    typename pcl::PointCloud<PointT>::Ptr getPointCloudWithIndices(
        const pcl::PointCloud<PointT>& input, const std::vector<int>& indices)
    {
        typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
        pcl::copyPointCloud(input, indices, *output);
        return output;
    }
 
    template <class PointT>
    typename pcl::PointCloud<PointT>::Ptr getPointCloudWithIndices(
        const pcl::PointCloud<PointT>& input, const pcl::PointIndices& indices)
    {
        typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
        pcl::copyPointCloud(input, indices.indices, *output);
        return output;
    }
 
 
    inline bool comparePointClustersDescending(const pcl::PointIndices& lhs, const pcl::PointIndices& rhs)
    {
        return lhs.indices.size() > rhs.indices.size();
    }
 
    inline void sortPointClustersDescending(std::vector<pcl::PointIndices>& clusterIndices)
    {
        std::sort(clusterIndices.begin(), clusterIndices.end(), comparePointClustersDescending);
    }
 
    template <class PointT>
    inline void filterNaNs(pcl::PointCloud<PointT>& pc)
    {
        const auto isNan = [](const PointT& p)->bool{
          return std::isnan(p.z);
        };
 
        pc.points.erase(std::remove_if(pc.points.begin(), pc.points.end(), isNan), pc.points.end());
 
        // Needed?
        pc.width = pc.points.size();
        pc.height = 1;
        pc.is_dense = true;
    }
 
}