Merge pull request #1708 from alicevision/dev/lidarSimplification

Lidar decimation and merging

src/aliceVision/fuseCut/InputSet.cpp

@@ -5,9 +5,48 @@
 // You can obtain one at https://mozilla.org/MPL/2.0/.
 
 #include "InputSet.hpp"
-
 #include <iostream>
 
+namespace Eigen {
+
+template<typename T, int M, int N>
+Eigen::Matrix<T, M, N> tag_invoke(boost::json::value_to_tag<Eigen::Matrix<T, M, N>>, boost::json::value const& jv)
+{
+    Eigen::Matrix<T, M, N> ret;
+
+    std::vector<T> buf = boost::json::value_to<std::vector<T>>(jv);
+
+    int pos = 0;
+    for (int i = 0; i < M; i++)
+    {
+        for (int j = 0; j < N; j++)
+        {
+            ret(i, j) = buf[pos];
+            pos++;
+        }
+    }
+
+    return ret;
+}
+
+template<typename T, int M, int N>
+void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, Eigen::Matrix<T, M, N> const& input)
+{
+    std::vector<T> buf;
+
+    for (int i = 0; i < M; i++)
+    {
+        for (int j = 0; j < N; j++)
+        {
+            buf.push_back(input(i, j));
+        }
+    }
+
+    jv = boost::json::value_from<std::vector<T>>(std::move(buf));
+}
+
+}  // namespace Eigen
+
 namespace aliceVision {
 namespace fuseCut {
 
@@ -18,6 +57,8 @@ Input tag_invoke(boost::json::value_to_tag<Input>, boost::json::value const& jv)
     Input ret;
     ret.sfmPath = boost::json::value_to<std::string>(obj.at("sfmPath"));
     ret.subMeshPath = boost::json::value_to<std::string>(obj.at("subMeshPath"));
+    ret.bbMin = boost::json::value_to<Eigen::Vector3d>(obj.at("bbMin"));
+    ret.bbMax = boost::json::value_to<Eigen::Vector3d>(obj.at("bbMax"));
 
     return ret;
 }
@@ -27,6 +68,8 @@ void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, Inpu
     jv = {
       {"sfmPath", t.sfmPath},
       {"subMeshPath", t.subMeshPath},
+      {"bbMin", boost::json::value_from(t.bbMin)},
+      {"bbMax", boost::json::value_from(t.bbMax)},
     };
 }
 

src/aliceVision/fuseCut/InputSet.hpp

@@ -7,6 +7,7 @@
 #pragma once
 
 #include <boost/json.hpp>
+#include <Eigen/Dense>
 
 namespace aliceVision {
 namespace fuseCut {
@@ -15,6 +16,8 @@ struct Input
 {
     std::string sfmPath;
     std::string subMeshPath;
+    Eigen::Vector3d bbMin;
+    Eigen::Vector3d bbMax;
 };
 
 using InputSet = std::vector<Input>;
@@ -30,4 +33,4 @@ Input tag_invoke(boost::json::value_to_tag<Input>, boost::json::value const& jv)
 void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, Input const& t);
 
 }  // namespace fuseCut
-}  // namespace aliceVision
+}  // namespace aliceVision

src/aliceVision/geometry/Intersection.cpp

@@ -46,5 +46,167 @@ bool rayIntersectUnitSphere(Vec3& coordinates, const Vec3& start, const Vec3& di
     return true;
 }
 
+bool rayIntersectAABB(const Eigen::Vector3d& bbMin,
+                      const Eigen::Vector3d& bbMax,
+                      const Eigen::Vector3d& start,
+                      const Eigen::Vector3d& direction,
+                      double& boundsMin,
+                      double& boundsMax)
+{
+    // x+lambda*dx>bbmin.x
+    // y+lambda*dy>bbmin.y
+    // z+lambda*dz>bbmin.z
+    // x+lambda*dx<bbmax.x
+    // y+lambda*dy<bbmax.y
+    // z+lambda*dz<bbmax.z
+
+    // lambda > (bbmin.x-x)/dx
+    // lambda > (bbmin.y-y)/dy
+    // lambda > (bbmin.z-z)/dz
+    // lambda < (bbmax.x-x)/dx
+    // lambda < (bbmax.y-y)/dy
+    // lambda < (bbmax.z-z)/dz
+
+    boundsMin = std::numeric_limits<double>::lowest();
+    boundsMax = std::numeric_limits<double>::max();
+
+    if (std::abs(direction.x()) > 1e-12)
+    {
+        double lmin = (bbMin.x() - start.x()) / direction.x();
+        double lmax = (bbMax.x() - start.x()) / direction.x();
+
+        if (direction.x() > 0.0)
+        {
+            boundsMin = std::max(boundsMin, lmin);
+            boundsMax = std::min(boundsMax, lmax);
+        }
+        else
+        {
+            boundsMin = std::max(boundsMin, lmax);
+            boundsMax = std::min(boundsMax, lmin);
+        }
+    }
+    else
+    {
+        if (start.x() < bbMin.x() || start.x() > bbMax.x())
+        {
+            return false;
+        }
+    }
+
+    if (std::abs(direction.y()) > 1e-12)
+    {
+        double lmin = (bbMin.y() - start.y()) / direction.y();
+        double lmax = (bbMax.y() - start.y()) / direction.y();
+
+        if (direction.y() > 0.0)
+        {
+            boundsMin = std::max(boundsMin, lmin);
+            boundsMax = std::min(boundsMax, lmax);
+        }
+        else
+        {
+            boundsMin = std::max(boundsMin, lmax);
+            boundsMax = std::min(boundsMax, lmin);
+        }
+    }
+    else
+    {
+        if (start.y() < bbMin.y() || start.y() > bbMax.y())
+        {
+            return false;
+        }
+    }
+
+    if (std::abs(direction.z()) > 1e-12)
+    {
+        double lmin = (bbMin.z() - start.z()) / direction.z();
+        double lmax = (bbMax.z() - start.z()) / direction.z();
+
+        if (direction.z() > 0.0)
+        {
+            boundsMin = std::max(boundsMin, lmin);
+            boundsMax = std::min(boundsMax, lmax);
+        }
+        else
+        {
+            boundsMin = std::max(boundsMin, lmax);
+            boundsMax = std::min(boundsMax, lmin);
+        }
+    }
+    else
+    {
+        if (start.z() < bbMin.z() || start.z() > bbMax.z())
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool isSegmentIntersectAABB(const Eigen::Vector3d& bbMin, const Eigen::Vector3d& bbMax, const Eigen::Vector3d& start, const Eigen::Vector3d& end)
+{
+    double boundsMin, boundsMax;
+
+    // Compute direction
+    Eigen::Vector3d direction = end - start;
+    double scale = direction.norm();
+    if (scale < 1e-12)
+    {
+        return false;
+    }
+    direction = direction / scale;
+
+    if (!rayIntersectAABB(bbMin, bbMax, start, direction, boundsMin, boundsMax))
+    {
+        return false;
+    }
+
+    if (boundsMax < 0.0)
+    {
+        return false;
+    }
+
+    if (boundsMax > scale)
+    {
+        return false;
+    }
+
+    return (boundsMin < boundsMax);
+}
+
+bool intersectionBetweenAABB(const Eigen::Vector3d& inputbbMin1,
+                             const Eigen::Vector3d& inputbbMax1,
+                             const Eigen::Vector3d& inputbbMin2,
+                             const Eigen::Vector3d& inputbbMax2,
+                             Eigen::Vector3d& bbMin,
+                             Eigen::Vector3d& bbMax)
+{
+    bbMin.x() = std::max(inputbbMin1.x(), inputbbMin2.x());
+    bbMax.x() = std::min(inputbbMax1.x(), inputbbMax2.x());
+    bbMin.y() = std::max(inputbbMin1.y(), inputbbMin2.y());
+    bbMax.y() = std::min(inputbbMax1.y(), inputbbMax2.y());
+    bbMin.z() = std::max(inputbbMin1.z(), inputbbMin2.z());
+    bbMax.z() = std::min(inputbbMax1.z(), inputbbMax2.z());
+
+    if (bbMin.x() >= bbMax.x())
+    {
+        return false;
+    }
+
+    if (bbMin.y() >= bbMax.y())
+    {
+        return false;
+    }
+
+    if (bbMin.z() >= bbMax.z())
+    {
+        return false;
+    }
+
+    return true;
+}
+
 }  // namespace geometry
 }  // namespace aliceVision

src/aliceVision/geometry/Intersection.hpp

@@ -7,18 +7,87 @@
 #pragma once
 
 #include <aliceVision/numeric/numeric.hpp>
+#include <Eigen/Dense>
 
 namespace aliceVision {
 namespace geometry {
 
 /**
- * @brief find the intersection of a ray (composed of a starting point and a direction) with the unit sphere centered on (0,0,0).
+ * @brief Find the intersection of a ray (composed of a starting point and a direction) with the unit sphere centered on (0,0,0).
  * @param coordinates the coordinates of the intersection closest to the starting point
  * @param start the starting point of the ray
  * @param direction the direction of the ray
  * @return true if an intersection is found
  */
 bool rayIntersectUnitSphere(Vec3& coordinates, const Vec3& start, const Vec3& direction);
 
+/**
+ * @brief Check if a point is inside an axis aligned bounding box
+ * @param bbMin the minimal values of the bounding box on each axis
+ * @param bbMax the maximal values of the bounding box on each axis
+ * @return true if the point is inside the bounding box
+ */
+inline bool isPointInsideAABB(const Eigen::Vector3d& bbMin, const Eigen::Vector3d& bbMax, const Eigen::Vector3d& pt)
+{
+    if (pt.x() + 1e-12 < bbMin.x())
+        return false;
+    if (pt.y() + 1e-12 < bbMin.y())
+        return false;
+    if (pt.z() + 1e-12 < bbMin.z())
+        return false;
+    if (pt.x() - 1e-12 > bbMax.x())
+        return false;
+    if (pt.y() - 1e-12 > bbMax.y())
+        return false;
+    if (pt.z() - 1e-12 > bbMax.z())
+        return false;
+
+    return true;
+}
+
+/**
+ * @brief Compute the two intersections of the rays with the bounding box
+ * @param bbMin the minimal values of the bounding box on each axis
+ * @param bbMax the maximal values of the bounding box on each axis
+ * @param start the starting coordinates for the ray
+ * @param direction the direction of the ray
+ * @param boundsMin the scale of the direction vector for which the ray enters the bounding box
+ * @param boundsMin the scale of the direction vector for which the ray leaves the bounding box
+ * @return false if the ray does not intersect the bounding box
+ */
+bool rayIntersectAABB(const Eigen::Vector3d& bbMin,
+                      const Eigen::Vector3d& bbMax,
+                      const Eigen::Vector3d& start,
+                      const Eigen::Vector3d& direction,
+                      double& boundsMin,
+                      double& boundsMax);
+
+/**
+ * @brief Is my segment intersecting the bounding box?
+ * @param bbMin the minimal values of the bounding box on each axis
+ * @param bbMax the maximal values of the bounding box on each axis
+ * @param start the starting coordinates for the segment
+ * @param end the ending coordinates for the segment
+ * @return true if the segment intersect the bounding box
+ */
+bool isSegmentIntersectAABB(const Eigen::Vector3d& bbMin, const Eigen::Vector3d& bbMax, const Eigen::Vector3d& start, const Eigen::Vector3d& end);
+
+/**
+ * @brief Is my segment intersecting the bounding box?
+ * @param inputbbMin1 the minimal values of the first bounding box on each axis
+ * @param inputbbMax1 the maximal values of the first bounding box on each axis
+ * @param inputbbMin2 the minimal values of the second bounding box on each axis
+ * @param inputbbMax2 the maximal values of the second bounding box on each axis
+ * @param bbMin the minimal values of the bounding box on each axis which is the intersection of both bounding box
+ * @param bbMax the maximal values of the bounding box on each axis which is the intersection of both bounding box
+ * @return true if the bounding box intersects
+ */
+bool intersectionBetweenAABB(const Eigen::Vector3d& inputbbMin1,
+                             const Eigen::Vector3d& inputbbMax1,
+                             const Eigen::Vector3d& inputbbMin2,
+                             const Eigen::Vector3d& inputbbMax2,
+                             Eigen::Vector3d& bbMin,
+                             Eigen::Vector3d& bbMax);
+
 }  // namespace geometry
 }  // namespace aliceVision

src/aliceVision/mesh/CMakeLists.txt

@@ -10,6 +10,9 @@ set(mesh_files_headers
   meshVisibility.hpp
   Texturing.hpp
   UVAtlas.hpp
+  ModQuadricMetricT.hpp
+  QuadricMetricT.hpp
+
 )
 
 # Sources
@@ -23,6 +26,7 @@ set(mesh_files_sources
   meshVisibility.cpp
   Texturing.cpp
   UVAtlas.cpp
+  ModQuadricMetricT.cpp
 )
 
 alicevision_add_library(aliceVision_mesh
@@ -35,5 +39,6 @@ alicevision_add_library(aliceVision_mesh
   PRIVATE_LINKS
     aliceVision_system
     Boost::boost
+    OpenMeshCore
 )