Add convex hull-based filtering classes and unit tests

GNSS-Stylus.pro

@@ -65,6 +65,7 @@ SOURCES += \
     LivoxMid360/livoxmid360pointcloudandimudata.cpp \
     LivoxMid360/livoxmid360thread.cpp \
     PostProcessing/EasyEXIF/exif.cpp \
+    PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.cpp \
     PostProcessing/Lidar/PointFilter/expressionfilter.cpp \
     PostProcessing/Lidar/PointFilter/tinyexpr-plusplus/tinyexpr.cpp \
     PostProcessing/Lidar/PointFilter/tinyexprcustomfunctions.cpp \
@@ -108,6 +109,8 @@ HEADERS += \
     LivoxMid360/livoxmid360pointcloudandimudata.h \
     LivoxMid360/livoxmid360thread.h \
     PostProcessing/EasyEXIF/exif.h \
+    PostProcessing/Lidar/PointFilter/ConvexHull/3d-quickhull/quickhull.h \
+    PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.h \
     PostProcessing/Lidar/PointFilter/expressionfilter.h \
     PostProcessing/Lidar/PointFilter/lazyevaluator.h \
     PostProcessing/Lidar/PointFilter/tinyexpr-plusplus/tinyexpr.h \

PostProcessing/Lidar/PointFilter/ConvexHull/3d-quickhull/quickhull.h

@@ -86,8 +86,8 @@ void qh_free_mesh(qh_mesh_t mesh);
 
 #endif // QUICKHULL_H
 
-//#ifdef QUICKHULL_IMPLEMENTATION
-#if 1
+#ifdef QUICKHULL_IMPLEMENTATION
+//#if 1
 
 #include <math.h>   // sqrt & fabs
 #include <stdio.h>  // FILE

PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.cpp

@@ -0,0 +1,166 @@
+#include "convexhull.h"
+
+#define QUICKHULL_IMPLEMENTATION
+#include "3d-quickhull/quickhull.h"
+
+ConvexHull::ConvexHull()
+{
+    meshGenerated = false;
+}
+
+ConvexHull::~ConvexHull()
+{
+    freeGeneratedMesh();
+}
+
+void ConvexHull::freeGeneratedMesh(void)
+{
+    if (meshGenerated)
+    {
+        qh_free_mesh(mesh);
+        meshGenerated = false;
+    }
+}
+
+bool ConvexHull::generateMesh(void)
+{
+    if (!meshGenerated)
+    {
+        if (points.size() < 4)
+        {
+            return false;
+        }
+    }
+
+    qh_vertex_t* vertices = new qh_vertex_t[points.size()];
+
+    for (int i = 0; i < points.size(); i++)
+    {
+        vertices[i].x = points[i].x();
+        vertices[i].y = points[i].y();
+        vertices[i].z = points[i].z();
+    }
+
+    mesh = qh_quickhull3d(vertices, points.size());
+
+    delete[] vertices;
+
+    meshGenerated = true;
+    return true;
+}
+
+unsigned int ConvexHull::addPoints(const QVector<Eigen::Vector3d>& newPoints)
+{
+    unsigned int count = 0;
+
+    for (int i = 0; i < newPoints.size(); i++)
+    {
+        if (isPointUnique(newPoints[i]))
+        {
+            // Only add unique points
+            continue;
+        }
+        else
+        {
+            points.push_back(newPoints[i]);
+            count++;
+        }
+    }
+
+    freeGeneratedMesh();
+    return count;
+}
+
+bool ConvexHull::addPoint(const Eigen::Vector3d& newPoint)
+{
+    if (isPointUnique(newPoint))
+    {
+        // Only add unique points
+        return false;
+    }
+
+    points.push_back(newPoint);
+    freeGeneratedMesh();
+    return true;
+}
+
+void ConvexHull::clearPoints(void)
+{
+    points.clear();
+    freeGeneratedMesh();
+}
+
+bool ConvexHull::getFilter(ConvexHull::Filter& filter)
+{
+    filter.init();
+
+    if (!generateMesh())
+    {
+        return false;
+    }
+
+    double minX = std::numeric_limits<double>::max();
+    double maxX = std::numeric_limits<double>::min();
+    double minY = std::numeric_limits<double>::max();
+    double maxY = std::numeric_limits<double>::min();
+    double minZ = std::numeric_limits<double>::max();
+    double maxZ = std::numeric_limits<double>::min();
+
+    // find AABB (axis aligned bounding box)
+
+    for (unsigned int i = 0; i < mesh.nvertices; i++)
+    {
+        minX = std::min(minX, (double)mesh.vertices[i].x);
+        maxX = std::max(maxX, (double)mesh.vertices[i].x);
+
+        minY = std::min(minY, (double)mesh.vertices[i].y);
+        maxY = std::max(maxY, (double)mesh.vertices[i].y);
+
+        minZ = std::min(minZ, (double)mesh.vertices[i].z);
+        maxZ = std::max(maxZ, (double)mesh.vertices[i].z);
+    }
+
+    Eigen::AlignedBox3d newAABB(Eigen::Vector3d(minX, minY, minZ), Eigen::Vector3d(maxX, maxY, maxZ));
+    filter.aabb = newAABB;
+
+    for (unsigned int i = 0; i < mesh.nvertices / 3; i++)
+    {
+        Eigen::Vector3d pointA(mesh.vertices[i * 3].x, mesh.vertices[i * 3].y, mesh.vertices[i * 3].z);
+        Eigen::Vector3d pointB(mesh.vertices[i * 3 + 1].x, mesh.vertices[i * 3 + 1].y, mesh.vertices[i * 3 + 1].z);
+        Eigen::Vector3d pointC(mesh.vertices[i * 3 + 2].x, mesh.vertices[i * 3 + 2].y, mesh.vertices[i * 3 + 2].z);
+
+        Eigen::Vector3d vecAB = pointB - pointA;
+        Eigen::Vector3d vecAC = pointC - pointA;
+        Eigen::Vector3d faceNormal = vecAB.cross(vecAC).normalized();
+
+        Filter::FaceDef newFace;
+
+        newFace.origin = (1.0 / 3.0) * (pointA + pointB + pointC);
+        newFace.normal = faceNormal;
+
+        filter.faceDefs.push_back(newFace);
+    }
+
+    return true;
+}
+
+bool ConvexHull::isPointUnique(const Eigen::Vector3d& newPoint)
+{
+    return points.contains(newPoint);
+}
+
+unsigned int ConvexHull::getNumOfUniquePoints(void)
+{
+    return points.size();
+}
+
+void ConvexHull::exportHullToObjFile(const QString& filename)
+{
+    generateMesh();
+    qh_mesh_export(&mesh, filename.toLocal8Bit());
+}
+
+
+
+
+

PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.h

@@ -0,0 +1,69 @@
+#ifndef CONVEXHULL_H
+#define CONVEXHULL_H
+
+#include "QVector"
+#include "Eigen/Geometry"
+#include "3d-quickhull/quickhull.h"
+
+class ConvexHull
+{
+public:
+    class Filter
+    {
+    private:
+        class FaceDef
+        {
+        public:
+            Eigen::Vector3d origin;
+            Eigen::Vector3d normal;
+        };
+
+        Eigen::AlignedBox3d aabb;
+
+    public:
+        bool isInside(const Eigen::Vector3d& pointCoords)
+        {
+            if (!aabb.contains(pointCoords) || (faceDefs.size() < 4))
+            {
+                return false;
+            }
+
+            for (const FaceDef& face : qAsConst(faceDefs))
+            {
+                if ((pointCoords - face.origin).dot(face.normal) > 0)
+                {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+
+        bool isValid(void) { return faceDefs.size() >= 4; };
+
+    private:
+        QVector<FaceDef> faceDefs;
+        void init(void) { faceDefs.clear(); };
+        friend class ConvexHull;
+    };
+
+public:
+    ConvexHull();
+    ~ConvexHull();
+    unsigned int addPoints(const QVector<Eigen::Vector3d>& newPoints); // Returns the number of points added (only adds unique points)
+    bool addPoint(const Eigen::Vector3d& newPoint); // Returns true if point was added (only adds unique points)
+    void clearPoints(void);
+    bool getFilter(ConvexHull::Filter& filter);
+    bool isPointUnique(const Eigen::Vector3d& newPoint);
+    unsigned int getNumOfUniquePoints(void);
+    void exportHullToObjFile(const QString& filename);
+
+private:
+    QVector<Eigen::Vector3d> points;
+    qh_mesh mesh;
+    bool meshGenerated;
+    void freeGeneratedMesh(void);
+    bool generateMesh(void);
+};
+
+#endif // CONVEXHULL_H

UnitTests/UnitTests.pro

@@ -11,19 +11,24 @@ QMAKE_CXXFLAGS += --coverage
 QMAKE_LFLAGS += --coverage
 
 SOURCES +=  tst_lidarfiltering.cpp \
+    ../PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.cpp \
     ../PostProcessing/Lidar/PointFilter/expressionfilter.cpp \
     ../PostProcessing/Lidar/PointFilter/tinyexpr-plusplus/tinyexpr.cpp \
     ../PostProcessing/Lidar/PointFilter/tinyexprcustomfunctions.cpp \
     ../RPLidar/rplidarplausibilityfilter.cpp \
+    tst_convexhull.cpp \
     tst_epressionfilter.cpp \
     tst_lazyevaluator.cpp \
     tst_main.cpp
 
 HEADERS += \
+    ../PostProcessing/Lidar/PointFilter/ConvexHull/3d-quickhull/quickhull.h \
+    ../PostProcessing/Lidar/PointFilter/ConvexHull/convexhull.h \
     ../PostProcessing/Lidar/PointFilter/expressionfilter.h \
     ../PostProcessing/Lidar/PointFilter/lazyevaluator.h \
     ../PostProcessing/Lidar/PointFilter/tinyexpr-plusplus/tinyexpr.h \
     ../PostProcessing/Lidar/PointFilter/tinyexprcustomfunctions.h \
+    tst_convexhull.h \
     tst_expressionfilter.h \
     tst_lazyevaluator.h \
     tst_lidarfiltering.h