File UniformAreaSampler.cpp

File List > algos > mesh_to_pointcloud > UniformAreaSampler.cpp

Go to the documentation of this file

#include "mesh_to_pointcloud/UniformAreaSampler.h"

namespace Argos {

    double UniformAreaSampler::computeMeshSurfaceArea(const Mesh& mesh) const
    {
        const auto& faces = mesh.getFaces();
        const auto& vertices = mesh.getVertices();

        double totalArea = 0.0;

        for (const Face& face : faces)
        {
            const auto& indices = face.getIndices();

            if (indices.size() < 3)
                continue;

            for (std::size_t i = 1; i < indices.size() - 1; ++i)
            {
                const auto& A = vertices[indices[0]];
                const auto& B = vertices[indices[i]];
                const auto& C = vertices[indices[i + 1]];

                auto AB = B - A;
                auto AC = C - A;

                double area = 0.5 * (AB ^ AC).norm();
                totalArea += area;
            }
        }

        //std::cout << "Total area: " << totalArea << std::endl; // DEBUG

        return totalArea;
    }

    std::vector<std::size_t> UniformAreaSampler::distributePointsOverFaces(const Mesh& mesh, double totalArea) const 
    {
        const auto& faces = mesh.getFaces();
        const auto& vertices = mesh.getVertices();

        std::vector<double> faceAreas(faces.size(), 0.0);
        std::vector<std::size_t> pointsPerFace(faces.size(), 0);

        //double totalAreaComputed = 0.0; // DEBUG

        if (totalArea == 0.0)
            return pointsPerFace;

        // Calcul des aires par face
        for (std::size_t f = 0; f < faces.size(); ++f)
        {
            const auto& indices = faces[f].getIndices();

            if (indices.size() < 3)
                continue;

            double area = 0.0;

            for (std::size_t i = 1; i < indices.size() - 1; ++i)
            {
                const auto& A = vertices[indices[0]];
                const auto& B = vertices[indices[i]];
                const auto& C = vertices[indices[i + 1]];

                auto AB = B - A;
                auto AC = C - A;

                area += 0.5 * (AB ^ AC).norm();  // on additionne les aires des triangles de la face
            }

            faceAreas[f] = area;  // on stocke l'aire totale de la face courrente 

            //totalAreaComputed += area; // DEBUG
        }

        //std::cout << "Total area computed: " << totalAreaComputed << std::endl; // DEBUG

        // Repartition proportionnelle
        std::size_t assigned = 0;

        for (std::size_t f = 0; f < faces.size(); ++f)
        {
            std::size_t count = static_cast<std::size_t>(m_pointCount * (faceAreas[f] / totalArea));

            pointsPerFace[f] = count;
            //std::cout << "Face " << f << ": area = " << faceAreas[f] << ", points assigned = " << count << std::endl; // DEBUG
            assigned += count;
        }

        // Distribution du reste
        std::size_t remainder = m_pointCount - assigned;

        for (std::size_t f = 0; f < faces.size() && remainder > 0; ++f)
        {
            if (faceAreas[f] > 0.0)
            {
                ++pointsPerFace[f];
                --remainder;
            }
        }

        return pointsPerFace;
    }

    void UniformAreaSampler::sampleTriangleSurface(
        const Vector3D<double>& A,
        const Vector3D<double>& B,
        const Vector3D<double>& C,
        std::size_t pointCount,
        PointCloud& cloud
    ) const
    {
        for (std::size_t i = 0; i < pointCount; ++i)
        {
            double u = static_cast<double>(std::rand()) / RAND_MAX;
            double v = static_cast<double>(std::rand()) / RAND_MAX;

            if (u + v > 1.0)
            {
                u = 1.0 - u;
                v = 1.0 - v;
            }

            double alpha = 1.0 - u - v;
            double beta = u;
            double gamma = v;

            auto P = A * alpha + B * beta + C * gamma;
            cloud.addPoint(P);
        }
    }

    void UniformAreaSampler::sampleFaceSurface(
        const Mesh& mesh,
        const Face& face,
        std::size_t pointCount,
        PointCloud& cloud
    ) const
    {
        const auto& vertices = mesh.getVertices();
        const auto& indices = face.getIndices();

        if (indices.size() < 3)
            return;

        for (std::size_t i = 0; i < pointCount; ++i)
        {
            std::size_t triIndex = 1 + std::rand() % (indices.size() - 2);

            const auto& A = vertices[indices[0]];
            const auto& B = vertices[indices[triIndex]];
            const auto& C = vertices[indices[triIndex + 1]];

            sampleTriangleSurface(A, B, C, 1, cloud);
        }
    }

    PointCloud UniformAreaSampler::sample(const Mesh& mesh)
    {
        PointCloud cloud;
        cloud.reserve(m_pointCount);

        const auto& faces = mesh.getFaces();
        //std::cout << "Faces: " << faces.size() << std::endl; // DEBUG

        if (faces.empty() || m_pointCount == 0)
            return cloud;

        double totalArea = computeMeshSurfaceArea(mesh);
        //std::cout << "Total area: " << totalArea << std::endl; // DEBUG

        if (totalArea == 0.0)
            return cloud;

        // repartition des points par face
        std::vector<std::size_t> pointsPerFace = distributePointsOverFaces(mesh, totalArea);

        // echantillonnage face par face
        for (std::size_t f = 0; f < faces.size(); ++f)
        {
            if (pointsPerFace[f] == 0)
                continue;

            sampleFaceSurface(mesh, faces[f], pointsPerFace[f], cloud);
        }
        return cloud;
    }
}