tools.cpp

Go to the documentation of this file.

#include "tools.h"
 
#include <SimoxUtility/math/periodic/periodic_diff.h>
#include <SimoxUtility/math/periodic/periodic_mean.h>
 
Eigen::Vector3f
visionx::pointCloudMedian(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                          const pcl::PointIndices& indices)
{
    std::vector<float> xs, ys, zs;
    xs.reserve(cloud.size());
    ys.reserve(cloud.size());
    zs.reserve(cloud.size());
 
    auto processPoint = [&](const auto& p)
    {
        xs.push_back(p.x);
        ys.push_back(p.y);
        zs.push_back(p.z);
    };
 
    if (indices.indices.empty())
    {
        for (const auto& p : cloud)
        {
            processPoint(p);
        }
    }
    else
    {
        for (int i : indices.indices)
        {
            processPoint(cloud.at(size_t(i)));
        }
    }
 
    std::sort(xs.begin(), xs.end());
    std::sort(ys.begin(), ys.end());
    std::sort(zs.begin(), zs.end());
 
    size_t index = cloud.size() / 2;
    return Eigen::Vector3f(xs[index], ys[index], zs[index]);
}
 
Eigen::Hyperplane3f
visionx::fitPlaneSVD(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud, Eigen::Vector3f center)
{
    return fitPlaneSVD(cloud, pcl::PointIndices(), center);
}
 
Eigen::Hyperplane3f
visionx::fitPlaneSVD(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                     const pcl::PointIndices& indices,
                     Eigen::Vector3f center)
{
    /* Find SVD of A:
     * [                   ]
     * [ (c1-c) ... (cm-c) ] (3xm)
     * [                   ]
     *
     * Best normal is left singular vector of least singular value
     * according to: https://math.stackexchange.com/a/99317
     */
 
    // Build A
    using MatT = Eigen::Matrix<float, 3, Eigen::Dynamic>;
 
    MatT A;
    if (indices.indices.empty())
    {
        // A = Eigen::MatrixXf(3, cloud.size());
        A = MatT(3, cloud.size());
        for (size_t i = 0; i < cloud.size(); ++i)
        {
            A.col(int(i)) = Eigen::Vector3f(cloud[i].data) - center;
        }
    }
    else
    {
        int i = 0;
        A = MatT(3, indices.indices.size());
        for (int index : indices.indices)
        {
            A.col(i++) = Eigen::Vector3f(cloud.at(size_t(index)).data) - center;
        }
    }
 
    // Compute SVD
    Eigen::JacobiSVD<MatT> svd(A, Eigen::ComputeFullU);
 
    // A is 3xm => U is 3x3, V is mxm
    Eigen::Vector3f normal = svd.matrixU().col(2);
 
    return Eigen::Hyperplane3f(normal, center);
}
 
pcl::PointIndicesPtr
visionx::getPlaneInliers(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
                         Eigen::Hyperplane3f plane,
                         float maxDistance)
{
    pcl::PointIndicesPtr inliers(new pcl::PointIndices);
    for (size_t i = 0; i < cloud.size(); ++i)
    {
        if (plane.absDistance(Eigen::Vector3f(cloud[i].data)) <= maxDistance)
        {
            inliers->indices.push_back(int(i));
        }
    }
    return inliers;
}
 
float
visionx::hsvDistance(const pcl::PointXYZHSV& lhs,
                     const pcl::PointXYZHSV& rhs,
                     const Eigen::Vector3f& hsvWeights)
{
    return hsvDistance(
        Eigen::Vector3f(lhs.h, lhs.s, lhs.v), Eigen::Vector3f(rhs.h, rhs.s, rhs.v), hsvWeights);
}
 
float
visionx::hsvDistance(const Eigen::Vector3f& lhs,
                     const Eigen::Vector3f& rhs,
                     const Eigen::Vector3f& hsvWeights)
{
    Eigen::Array3f dists(simox::math::periodic_diff(lhs.x(), rhs.x(), 0.f, 360.f) / 180.f,
                         lhs.y() - rhs.y(),
                         lhs.z() - rhs.z());
    Eigen::Array3f weighted = hsvWeights.normalized().array() * dists.abs();
    return weighted.sum();
}
 
Eigen::Vector3f
visionx::hsvMean(const pcl::PointCloud<pcl::PointXYZHSV>& hsvCloud, const std::vector<int>& indices)
{
    // Average color.
    std::vector<float> hues;
    hues.reserve(indices.size());
    float s = 0, v = 0;
    for (int i : indices)
    {
        const auto& p = hsvCloud.at(size_t(i));
        hues.push_back(p.h);
        s += p.s;
        v += p.v;
    }
    float h = simox::math::periodic_mean(hues, 0.f, 360.f);
    Eigen::Vector3f hsv(h, s / indices.size(), v / indices.size());
    return hsv;
}
 
Eigen::Hyperplane3f
visionx::alignPlaneNormal(const Eigen::Hyperplane3f plane, const Eigen::Vector3f normal)
{
    return plane.normal().dot(normal) >= 0 ? plane
                                           : Eigen::Hyperplane3f(-plane.normal(), -plane.offset());
}