eps fix for iou3d

Summary: Fix EPS issue that causes numerical instabilities when boxes are very close

Reviewed By: kjchalup

Differential Revision: D38661465

fbshipit-source-id: d2b6753cba9dc2f0072ace5289c9aa815a1a29f6
This commit is contained in:
Georgia Gkioxari 2022-08-22 04:26:19 -07:00 committed by Facebook GitHub Bot
parent 06cbba2628
commit 1bfe6bf20a
5 changed files with 923 additions and 188 deletions

View File

@ -88,9 +88,9 @@ __global__ void IoUBox3DKernel(
for (int b1 = 0; b1 < box1_count; ++b1) {
for (int b2 = 0; b2 < box2_count; ++b2) {
const bool is_coplanar =
IsCoplanarFace(box1_intersect[b1], box2_intersect[b2]);
IsCoplanarTriTri(box1_intersect[b1], box2_intersect[b2]);
const float area = FaceArea(box1_intersect[b1]);
if ((is_coplanar) && (area > kEpsilon)) {
if ((is_coplanar) && (area > aEpsilon)) {
tri2_keep[b2].keep = false;
}
}

View File

@ -80,9 +80,9 @@ std::tuple<at::Tensor, at::Tensor> IoUBox3DCpu(
for (int b1 = 0; b1 < box1_intersect.size(); ++b1) {
for (int b2 = 0; b2 < box2_intersect.size(); ++b2) {
const bool is_coplanar =
IsCoplanarFace(box1_intersect[b1], box2_intersect[b2]);
IsCoplanarTriTri(box1_intersect[b1], box2_intersect[b2]);
const float area = FaceArea(box1_intersect[b1]);
if ((is_coplanar) && (area > kEpsilon)) {
if ((is_coplanar) && (area > aEpsilon)) {
tri2_keep[b2] = 0;
}
}

View File

@ -12,7 +12,15 @@
#include <cstdio>
#include "utils/float_math.cuh"
__constant__ const float kEpsilon = 1e-5;
// dEpsilon: Used in dot products and is used to assess whether two unit vectors
// are orthogonal (or coplanar). It's an epsilon on cos(θ).
// With dEpsilon = 0.001, two unit vectors are considered co-planar
// if their θ = 2.5 deg.
__constant__ const float dEpsilon = 1e-3;
// aEpsilon: Used once in main function to check for small face areas
__constant__ const float aEpsilon = 1e-4;
// kEpsilon: Used only for norm(u) = u/max(||u||, kEpsilon)
__constant__ const float kEpsilon = 1e-8;
/*
_PLANES and _TRIS define the 4- and 3-connectivity
@ -120,10 +128,37 @@ __device__ inline void GetBoxPlanes(
}
}
// The normal of the face defined by vertices (v0, v1, v2)
// Define e0 to be the edge connecting (v1, v0)
// Define e1 to be the edge connecting (v2, v0)
// normal is the cross product of e0, e1
// The normal of a plane spanned by vectors e0 and e1
//
// Args
// e0, e1: float3 vectors defining a plane
//
// Returns
// float3: normal of the plane
//
__device__ inline float3 GetNormal(const float3 e0, const float3 e1) {
float3 n = cross(e0, e1);
n = n / std::fmaxf(norm(n), kEpsilon);
return n;
}
// The center of a triangle defined by vertices (v0, v1, v2)
//
// Args
// v0, v1, v2: float3 coordinates of the vertices of the triangle
//
// Returns
// float3: center of the triangle
//
__device__ inline float3
TriCenter(const float3 v0, const float3 v1, const float3 v2) {
float3 ctr = (v0 + v1 + v2) / 3.0f;
return ctr;
}
// The normal of the triangle defined by vertices (v0, v1, v2)
// We find the "best" edges connecting the face center to the vertices,
// such that the cross product between the edges is maximized.
//
// Args
// v0, v1, v2: float3 coordinates of the vertices of the face
@ -132,9 +167,24 @@ __device__ inline void GetBoxPlanes(
// float3: normal for the face
//
__device__ inline float3
FaceNormal(const float3 v0, const float3 v1, const float3 v2) {
float3 n = cross(v1 - v0, v2 - v0);
n = n / fmaxf(norm(n), kEpsilon);
TriNormal(const float3 v0, const float3 v1, const float3 v2) {
const float3 ctr = TriCenter(v0, v1, v2);
const float d01 = norm(cross(v0 - ctr, v1 - ctr));
const float d02 = norm(cross(v0 - ctr, v2 - ctr));
const float d12 = norm(cross(v1 - ctr, v2 - ctr));
float3 n = GetNormal(v0 - ctr, v1 - ctr);
float max_dist = d01;
if (d02 > max_dist) {
max_dist = d02;
n = GetNormal(v0 - ctr, v2 - ctr);
}
if (d12 > max_dist) {
n = GetNormal(v1 - ctr, v2 - ctr);
}
return n;
}
@ -158,8 +208,75 @@ __device__ inline float FaceArea(const FaceVerts& tri) {
return norm(n) / 2.0;
}
// The normal of a box plane defined by the verts in `plane` with
// the centroid of the box given by `center`.
// The center of a plane defined by vertices (v0, v1, v2, v3)
//
// Args
// v0, v1, v2, v3: float3 coordinates of the vertices of the plane
//
// Returns
// float3: center of the plane
//
__device__ inline float3 PlaneCenter(
const float3 v0,
const float3 v1,
const float3 v2,
const float3 v3) {
float3 ctr = (v0 + v1 + v2 + v3) / 4.0f;
return ctr;
}
// The normal of a planar face with vertices (v0, v1, v2, v3)
// We find the "best" edges connecting the face center to the vertices,
// such that the cross product between the edges is maximized.
//
// Args
// e0, e1: float3 coordinates of the vertices of the plane
//
// Returns
// float3: center of the plane
//
__device__ inline float3 PlaneNormal(
const float3 v0,
const float3 v1,
const float3 v2,
const float3 v3) {
const float3 ctr = PlaneCenter(v0, v1, v2, v3);
const float d01 = norm(cross(v0 - ctr, v1 - ctr));
const float d02 = norm(cross(v0 - ctr, v2 - ctr));
const float d03 = norm(cross(v0 - ctr, v3 - ctr));
const float d12 = norm(cross(v1 - ctr, v2 - ctr));
const float d13 = norm(cross(v1 - ctr, v3 - ctr));
const float d23 = norm(cross(v2 - ctr, v3 - ctr));
float max_dist = d01;
float3 n = GetNormal(v0 - ctr, v1 - ctr);
if (d02 > max_dist) {
max_dist = d02;
n = GetNormal(v0 - ctr, v2 - ctr);
}
if (d03 > max_dist) {
max_dist = d03;
n = GetNormal(v0 - ctr, v3 - ctr);
}
if (d12 > max_dist) {
max_dist = d12;
n = GetNormal(v1 - ctr, v2 - ctr);
}
if (d13 > max_dist) {
max_dist = d13;
n = GetNormal(v1 - ctr, v3 - ctr);
}
if (d23 > max_dist) {
n = GetNormal(v2 - ctr, v3 - ctr);
}
return n;
}
// The normal of a box plane defined by the verts in `plane` such that it
// points toward the centroid of the box given by `center`.
//
// Args
// plane: float3 coordinates of the vertices of the plane
// center: float3 coordinates of the center of the box from
@ -173,23 +290,30 @@ template <typename FaceVertsPlane>
__device__ inline float3 PlaneNormalDirection(
const FaceVertsPlane& plane,
const float3& center) {
// Only need the first 3 verts of the plane
// The plane's vertices
const float3 v0 = plane.v0;
const float3 v1 = plane.v1;
const float3 v2 = plane.v2;
const float3 v3 = plane.v3;
// We project the center on the plane defined by (v0, v1, v2)
// We can write center = v0 + a * e0 + b * e1 + c * n
// The plane's center
const float3 plane_center = PlaneCenter(v0, v1, v2, v3);
// The plane's normal
float3 n = PlaneNormal(v0, v1, v2, v3);
// We project the center on the plane defined by (v0, v1, v2, v3)
// We can write center = plane_center + a * e0 + b * e1 + c * n
// We know that <e0, n> = 0 and <e1, n> = 0 and
// <a, b> is the dot product between a and b.
// This means we can solve for c as:
// c = <center - v0 - a * e0 - b * e1, n> = <center - v0, n>
float3 n = FaceNormal(v0, v1, v2);
const float c = dot((center - v0), n);
// c = <center - plane_center - a * e0 - b * e1, n>
// = <center - plane_center, n>
const float c = dot((center - plane_center), n);
// If c is negative, then we revert the direction of n such that n
// points "inside"
if (c < kEpsilon) {
if (c < 0.0f) {
n = -1.0f * n;
}
@ -318,16 +442,22 @@ __device__ inline float3 PolyhedronCenter(
//
__device__ inline bool
IsInside(const FaceVerts& plane, const float3& normal, const float3& point) {
// Get one vert of the plane
// Vertices of the plane
const float3 v0 = plane.v0;
const float3 v1 = plane.v1;
const float3 v2 = plane.v2;
const float3 v3 = plane.v3;
// Every point p can be written as p = v0 + a e0 + b e1 + c n
// The center of the plane
const float3 plane_ctr = PlaneCenter(v0, v1, v2, v3);
// Every point p can be written as p = plane_ctr + a e0 + b e1 + c n
// Solving for c:
// c = (point - v0 - a * e0 - b * e1).dot(n)
// c = (point - plane_ctr - a * e0 - b * e1).dot(n)
// We know that <e0, n> = 0 and <e1, n> = 0
// So the calculation can be simplified as:
const float c = dot((point - v0), normal);
const bool inside = c > -1.0f * kEpsilon;
const float c = dot((point - plane_ctr), normal);
const bool inside = c >= 0.0f;
return inside;
}
@ -348,20 +478,150 @@ __device__ inline float3 PlaneEdgeIntersection(
const float3& normal,
const float3& p0,
const float3& p1) {
// Get one vert of the plane
// Vertices of the plane
const float3 v0 = plane.v0;
const float3 v1 = plane.v1;
const float3 v2 = plane.v2;
const float3 v3 = plane.v3;
// The center of the plane
const float3 plane_ctr = PlaneCenter(v0, v1, v2, v3);
// The point of intersection can be parametrized
// p = p0 + a (p1 - p0) where a in [0, 1]
// We want to find a such that p is on plane
// <p - v0, n> = 0
const float top = dot(-1.0f * (p0 - v0), normal);
const float bot = dot(p1 - p0, normal);
const float a = top / bot;
const float3 p = p0 + a * (p1 - p0);
// <p - plane_ctr, n> = 0
float3 direc = p1 - p0;
direc = direc / fmaxf(norm(direc), kEpsilon);
float3 p = (p1 + p0) / 2.0f;
if (abs(dot(direc, normal)) >= dEpsilon) {
const float top = -1.0f * dot(p0 - plane_ctr, normal);
const float bot = dot(p1 - p0, normal);
const float a = top / bot;
p = p0 + a * (p1 - p0);
}
return p;
}
// Compute the most distant points between two sets of vertices
//
// Args
// verts1, verts2: list of float3 defining the list of vertices
//
// Returns
// v1m, v2m: float3 vectors of the most distant points
// in verts1 and verts2 respectively
//
__device__ inline std::tuple<float3, float3> ArgMaxVerts(
std::initializer_list<float3> verts1,
std::initializer_list<float3> verts2) {
auto v1m = float3();
auto v2m = float3();
float maxdist = -1.0f;
for (const auto& v1 : verts1) {
for (const auto& v2 : verts2) {
if (norm(v1 - v2) > maxdist) {
v1m = v1;
v2m = v2;
maxdist = norm(v1 - v2);
}
}
}
return std::make_tuple(v1m, v2m);
}
// Compute a boolean indicator for whether or not two faces
// are coplanar
//
// Args
// tri1, tri2: FaceVerts struct of the vertex coordinates of
// the triangle face
//
// Returns
// bool: whether or not the two faces are coplanar
//
__device__ inline bool IsCoplanarTriTri(
const FaceVerts& tri1,
const FaceVerts& tri2) {
// Get verts for face 1
const float3 v0_1 = tri1.v0;
const float3 v1_1 = tri1.v1;
const float3 v2_1 = tri1.v2;
const float3 tri1_ctr = TriCenter(v0_1, v1_1, v2_1);
const float3 tri1_n = TriNormal(v0_1, v1_1, v2_1);
// Get verts for face 2
const float3 v0_2 = tri2.v0;
const float3 v1_2 = tri2.v1;
const float3 v2_2 = tri2.v2;
const float3 tri2_ctr = TriCenter(v0_2, v1_2, v2_2);
const float3 tri2_n = TriNormal(v0_2, v1_2, v2_2);
// Check if parallel
const bool check1 = abs(dot(tri1_n, tri2_n)) > 1 - dEpsilon;
// Compute most distant points
auto argvs =
ArgMaxVerts({tri1.v0, tri1.v1, tri1.v2}, {tri2.v0, tri2.v1, tri2.v2});
const float3 v1m = std::get<0>(argvs);
const float3 v2m = std::get<1>(argvs);
float3 n12m = v1m - v2m;
n12m = n12m / fmaxf(norm(n12m), kEpsilon);
const bool check2 = (abs(dot(n12m, tri1_n)) < dEpsilon) ||
(abs(dot(n12m, tri2_n)) < dEpsilon);
return (check1 && check2);
}
// Compute a boolean indicator for whether or not a triangular and a planar
// face are coplanar
//
// Args
// tri, plane: FaceVerts struct of the vertex coordinates of
// the triangle and planar face
// normal: the normal direction of the plane pointing "inside"
//
// Returns
// bool: whether or not the two faces are coplanar
//
__device__ inline bool IsCoplanarTriPlane(
const FaceVerts& tri,
const FaceVerts& plane,
const float3& normal) {
// Get verts for tri
const float3 v0t = tri.v0;
const float3 v1t = tri.v1;
const float3 v2t = tri.v2;
const float3 tri_ctr = TriCenter(v0t, v1t, v2t);
const float3 nt = TriNormal(v0t, v1t, v2t);
// check if parallel
const bool check1 = abs(dot(nt, normal)) > 1 - dEpsilon;
// Compute most distant points
auto argvs = ArgMaxVerts(
{tri.v0, tri.v1, tri.v2}, {plane.v0, plane.v1, plane.v2, plane.v3});
const float3 v1m = std::get<0>(argvs);
const float3 v2m = std::get<1>(argvs);
float3 n12m = v1m - v2m;
n12m = n12m / fmaxf(norm(n12m), kEpsilon);
const bool check2 = abs(dot(n12m, normal)) < dEpsilon;
return (check1 && check2);
}
// Triangle is clipped into a quadrilateral
// based on the intersection points with the plane.
// Then the quadrilateral is divided into two triangles.
@ -477,6 +737,14 @@ __device__ inline int ClipTriByPlane(
const bool isin1 = IsInside(plane, normal, v1);
const bool isin2 = IsInside(plane, normal, v2);
// Check coplanar
const bool iscoplanar = IsCoplanarTriPlane(tri, plane, normal);
if (iscoplanar) {
// Return input vertices
face_verts_out[0] = {v0, v1, v2};
return 1;
}
// All in
if (isin0 && isin1 && isin2) {
// Return input vertices
@ -515,40 +783,6 @@ __device__ inline int ClipTriByPlane(
return 0;
}
// Compute a boolean indicator for whether or not two faces
// are coplanar
//
// Args
// tri1, tri2: FaceVerts struct of the vertex coordinates of
// the triangle face
//
// Returns
// bool: whether or not the two faces are coplanar
//
__device__ inline bool IsCoplanarFace(
const FaceVerts& tri1,
const FaceVerts& tri2) {
// Get verts for face 1
const float3 v0 = tri1.v0;
const float3 v1 = tri1.v1;
const float3 v2 = tri1.v2;
const float3 n1 = FaceNormal(v0, v1, v2);
int coplanar_count = 0;
// Check v0, v1, v2
if (abs(dot(tri2.v0 - v0, n1)) < kEpsilon) {
coplanar_count++;
}
if (abs(dot(tri2.v1 - v0, n1)) < kEpsilon) {
coplanar_count++;
}
if (abs(dot(tri2.v2 - v0, n1)) < kEpsilon) {
coplanar_count++;
}
return (coplanar_count == 3);
}
// Get the triangles from each box which are part of the
// intersecting polyhedron by computing the intersection
// points with each of the planes.

View File

@ -18,7 +18,15 @@
#include <type_traits>
#include "utils/vec3.h"
const auto kEpsilon = 1e-5;
// dEpsilon: Used in dot products and is used to assess whether two unit vectors
// are orthogonal (or coplanar). It's an epsilon on cos(θ).
// With dEpsilon = 0.001, two unit vectors are considered co-planar
// if their θ = 2.5 deg.
const auto dEpsilon = 1e-3;
// aEpsilon: Used once in main function to check for small face areas
const auto aEpsilon = 1e-4;
// kEpsilon: Used only for norm(u) = u/max(||u||, kEpsilon)
const auto kEpsilon = 1e-8;
/*
_PLANES and _TRIS define the 4- and 3-connectivity
@ -129,20 +137,108 @@ inline face_verts GetBoxPlanes(const Box& box) {
return box_planes;
}
// The normal of the face defined by vertices (v0, v1, v2)
// Define e0 to be the edge connecting (v1, v0)
// Define e1 to be the edge connecting (v2, v0)
// normal is the cross product of e0, e1
// The normal of a plane spanned by vectors e0 and e1
//
// Args
// v0, v1, v2: vec3 coordinates of the vertices of the face
// e0, e1: vec3 vectors defining a plane
//
// Returns
// vec3: normal of the plane
//
inline vec3<float> GetNormal(const vec3<float> e0, const vec3<float> e1) {
vec3<float> n = cross(e0, e1);
n = n / std::fmaxf(norm(n), kEpsilon);
return n;
}
// The center of a triangle tri
//
// Args
// tri: vec3 coordinates of the vertices of the triangle
//
// Returns
// vec3: center of the triangle
//
inline vec3<float> TriCenter(const std::vector<vec3<float>>& tri) {
// Vertices of the triangle
const vec3<float> v0 = tri[0];
const vec3<float> v1 = tri[1];
const vec3<float> v2 = tri[2];
return (v0 + v1 + v2) / 3.0f;
}
// The normal of the triangle defined by vertices (v0, v1, v2)
// We find the "best" edges connecting the face center to the vertices,
// such that the cross product between the edges is maximized.
//
// Args
// tri: vec3 coordinates of the vertices of the face
//
// Returns
// vec3: normal for the face
//
inline vec3<float> FaceNormal(vec3<float> v0, vec3<float> v1, vec3<float> v2) {
vec3<float> n = cross(v1 - v0, v2 - v0);
n = n / std::fmaxf(norm(n), kEpsilon);
inline vec3<float> TriNormal(const std::vector<vec3<float>>& tri) {
// Get center of triangle
const vec3<float> ctr = TriCenter(tri);
// find the "best" normal as cross product of edges from center
float max_dist = -1.0f;
vec3<float> n = {0.0f, 0.0f, 0.0f};
for (int i = 0; i < 2; ++i) {
for (int j = i + 1; j < 3; ++j) {
const float dist = norm(cross(tri[i] - ctr, tri[j] - ctr));
if (dist > max_dist) {
n = GetNormal(tri[i] - ctr, tri[j] - ctr);
}
}
}
return n;
}
// The center of a plane
//
// Args
// plane: vec3 coordinates of the vertices of the plane
//
// Returns
// vec3: center of the plane
//
inline vec3<float> PlaneCenter(const std::vector<vec3<float>>& plane) {
// Vertices of the plane
const vec3<float> v0 = plane[0];
const vec3<float> v1 = plane[1];
const vec3<float> v2 = plane[2];
const vec3<float> v3 = plane[3];
return (v0 + v1 + v2 + v3) / 4.0f;
}
// The normal of a planar face with vertices (v0, v1, v2, v3)
// We find the "best" edges connecting the face center to the vertices,
// such that the cross product between the edges is maximized.
//
// Args
// plane: vec3 coordinates of the vertices of the planar face
//
// Returns
// vec3: normal of the planar face
//
inline vec3<float> PlaneNormal(const std::vector<vec3<float>>& plane) {
// Get center of planar face
vec3<float> ctr = PlaneCenter(plane);
// find the "best" normal as cross product of edges from center
float max_dist = -1.0f;
vec3<float> n = {0.0f, 0.0f, 0.0f};
for (int i = 0; i < 3; ++i) {
for (int j = i + 1; j < 4; ++j) {
const float dist = norm(cross(plane[i] - ctr, plane[j] - ctr));
if (dist > max_dist) {
n = GetNormal(plane[i] - ctr, plane[j] - ctr);
}
}
}
return n;
}
@ -166,8 +262,9 @@ inline float FaceArea(const std::vector<vec3<float>>& tri) {
return norm(n) / 2.0;
}
// The normal of a box plane defined by the verts in `plane` with
// the centroid of the box given by `center`.
// The normal of a box plane defined by the verts in `plane` such that it
// points toward the centroid of the box given by `center`.
//
// Args
// plane: vec3 coordinates of the vertices of the plane
// center: vec3 coordinates of the center of the box from
@ -180,23 +277,22 @@ inline float FaceArea(const std::vector<vec3<float>>& tri) {
inline vec3<float> PlaneNormalDirection(
const std::vector<vec3<float>>& plane,
const vec3<float>& center) {
// Only need the first 3 verts of the plane
const vec3<float> v0 = plane[0];
const vec3<float> v1 = plane[1];
const vec3<float> v2 = plane[2];
// The plane's center & normal
const vec3<float> plane_center = PlaneCenter(plane);
vec3<float> n = PlaneNormal(plane);
// We project the center on the plane defined by (v0, v1, v2)
// We can write center = v0 + a * e0 + b * e1 + c * n
// We project the center on the plane defined by (v0, v1, v2, v3)
// We can write center = plane_center + a * e0 + b * e1 + c * n
// We know that <e0, n> = 0 and <e1, n> = 0 and
// <a, b> is the dot product between a and b.
// This means we can solve for c as:
// c = <center - v0 - a * e0 - b * e1, n> = <center - v0, n>
vec3<float> n = FaceNormal(v0, v1, v2);
const float c = dot((center - v0), n);
// c = <center - plane_center - a * e0 - b * e1, n>
// = <center - plane_center, n>
const float c = dot((center - plane_center), n);
// If c is negative, then we revert the direction of n such that n
// points "inside"
if (c < kEpsilon) {
if (c < 0.0f) {
n = -1.0f * n;
}
@ -308,16 +404,16 @@ inline bool IsInside(
const std::vector<vec3<float>>& plane,
const vec3<float>& normal,
const vec3<float>& point) {
// Get one vert of the plane
const vec3<float> v0 = plane[0];
// The center of the plane
const vec3<float> plane_ctr = PlaneCenter(plane);
// Every point p can be written as p = v0 + a e0 + b e1 + c n
// Every point p can be written as p = plane_ctr + a e0 + b e1 + c n
// Solving for c:
// c = (point - v0 - a * e0 - b * e1).dot(n)
// c = (point - plane_ctr - a * e0 - b * e1).dot(n)
// We know that <e0, n> = 0 and <e1, n> = 0
// So the calculation can be simplified as:
const float c = dot((point - v0), normal);
const bool inside = c > -1.0f * kEpsilon;
const float c = dot((point - plane_ctr), normal);
const bool inside = c >= 0.0f;
return inside;
}
@ -338,20 +434,124 @@ inline vec3<float> PlaneEdgeIntersection(
const vec3<float>& normal,
const vec3<float>& p0,
const vec3<float>& p1) {
// Get one vert of the plane
const vec3<float> v0 = plane[0];
// The center of the plane
const vec3<float> plane_ctr = PlaneCenter(plane);
// The point of intersection can be parametrized
// p = p0 + a (p1 - p0) where a in [0, 1]
// We want to find a such that p is on plane
// <p - v0, n> = 0
const float top = dot(-1.0f * (p0 - v0), normal);
const float bot = dot(p1 - p0, normal);
const float a = top / bot;
const vec3<float> p = p0 + a * (p1 - p0);
// <p - ctr, n> = 0
vec3<float> direc = p1 - p0;
direc = direc / std::fmaxf(norm(direc), kEpsilon);
vec3<float> p = (p1 + p0) / 2.0f;
if (std::abs(dot(direc, normal)) >= dEpsilon) {
const float top = -1.0f * dot(p0 - plane_ctr, normal);
const float bot = dot(p1 - p0, normal);
const float a = top / bot;
p = p0 + a * (p1 - p0);
}
return p;
}
// Compute the most distant points between two sets of vertices
//
// Args
// verts1, verts2: vec3 defining the list of vertices
//
// Returns
// v1m, v2m: vec3 vectors of the most distant points
// in verts1 and verts2 respectively
//
inline std::tuple<vec3<float>, vec3<float>> ArgMaxVerts(
const std::vector<vec3<float>>& verts1,
const std::vector<vec3<float>>& verts2) {
vec3<float> v1m = {0.0f, 0.0f, 0.0f};
vec3<float> v2m = {0.0f, 0.0f, 0.0f};
float maxdist = -1.0f;
for (const auto& v1 : verts1) {
for (const auto& v2 : verts2) {
if (norm(v1 - v2) > maxdist) {
v1m = v1;
v2m = v2;
maxdist = norm(v1 - v2);
}
}
}
return std::make_tuple(v1m, v2m);
}
// Compute a boolean indicator for whether or not two faces
// are coplanar
//
// Args
// tri1, tri2: std:vector<vec3> of the vertex coordinates of
// triangle faces
//
// Returns
// bool: whether or not the two faces are coplanar
//
inline bool IsCoplanarTriTri(
const std::vector<vec3<float>>& tri1,
const std::vector<vec3<float>>& tri2) {
// Get normal for tri 1
const vec3<float> n1 = TriNormal(tri1);
// Get normal for tri 2
const vec3<float> n2 = TriNormal(tri2);
// Check if parallel
const bool check1 = std::abs(dot(n1, n2)) > 1 - dEpsilon;
// Compute most distant points
auto argvs = ArgMaxVerts(tri1, tri2);
const auto [v1m, v2m] = argvs;
vec3<float> n12m = v1m - v2m;
n12m = n12m / std::fmaxf(norm(n12m), kEpsilon);
const bool check2 = (std::abs(dot(n12m, n1)) < dEpsilon) ||
(std::abs(dot(n12m, n2)) < dEpsilon);
return (check1 && check2);
}
// Compute a boolean indicator for whether or not a triangular and a planar
// face are coplanar
//
// Args
// tri, plane: std:vector<vec3> of the vertex coordinates of
// triangular face and planar face
// normal: the normal direction of the plane pointing "inside"
//
// Returns
// bool: whether or not the two faces are coplanar
//
inline bool IsCoplanarTriPlane(
const std::vector<vec3<float>>& tri,
const std::vector<vec3<float>>& plane,
const vec3<float>& normal) {
// Get normal for tri
const vec3<float> nt = TriNormal(tri);
// check if parallel
const bool check1 = std::abs(dot(nt, normal)) > 1 - dEpsilon;
// Compute most distant points
auto argvs = ArgMaxVerts(tri, plane);
const auto [v1m, v2m] = argvs;
vec3<float> n12m = v1m - v2m;
n12m = n12m / std::fmaxf(norm(n12m), kEpsilon);
const bool check2 = std::abs(dot(n12m, normal)) < dEpsilon;
return (check1 && check2);
}
// Triangle is clipped into a quadrilateral
// based on the intersection points with the plane.
// Then the quadrilateral is divided into two triangles.
@ -436,6 +636,14 @@ inline face_verts ClipTriByPlane(
const vec3<float> v1 = tri[1];
const vec3<float> v2 = tri[2];
// Check coplanar
const bool iscoplanar = IsCoplanarTriPlane(tri, plane, normal);
if (iscoplanar) {
// Return input vertices
face_verts tris = {{v0, v1, v2}};
return tris;
}
// Check each of the triangle vertices to see if it is inside the plane
const bool isin0 = IsInside(plane, normal, v0);
const bool isin1 = IsInside(plane, normal, v1);
@ -480,35 +688,6 @@ inline face_verts ClipTriByPlane(
return empty_tris;
}
// Compute a boolean indicator for whether or not two faces
// are coplanar
//
// Args
// tri1, tri2: std:vector<vec3> of the vertex coordinates of
// triangle faces
//
// Returns
// bool: whether or not the two faces are coplanar
//
inline bool IsCoplanarFace(
const std::vector<vec3<float>>& tri1,
const std::vector<vec3<float>>& tri2) {
// Get verts for face 1
const vec3<float> v0 = tri1[0];
const vec3<float> v1 = tri1[1];
const vec3<float> v2 = tri1[2];
const vec3<float> n1 = FaceNormal(v0, v1, v2);
int coplanar_count = 0;
for (int i = 0; i < 3; ++i) {
float d = std::abs(dot(tri2[i] - v0, n1));
if (d < kEpsilon) {
coplanar_count = coplanar_count + 1;
}
}
return (coplanar_count == 3);
}
// Get the triangles from each box which are part of the
// intersecting polyhedron by computing the intersection
// points with each of the planes.

View File

@ -21,7 +21,8 @@ from .common_testing import get_random_cuda_device, get_tests_dir, TestCaseMixin
OBJECTRON_TO_PYTORCH3D_FACE_IDX = [0, 4, 6, 2, 1, 5, 7, 3]
DATA_DIR = get_tests_dir() / "data"
DEBUG = False
EPS = 1e-5
DOT_EPS = 1e-3
AREA_EPS = 1e-4
UNIT_BOX = [
[0, 0, 0],
@ -457,6 +458,207 @@ class TestIoU3D(TestCaseMixin, unittest.TestCase):
self.assertClose(
iou, torch.tensor([[0.91]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# symmetry
vol, iou = overlap_fn(box15b[None], box15a[None])
self.assertClose(
iou, torch.tensor([[0.91]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# 16th test: From GH issue 1287
box16a = torch.tensor(
[
[-167.5847, -70.6167, -2.7927],
[-166.7333, -72.4264, -2.7927],
[-166.7333, -72.4264, -4.5927],
[-167.5847, -70.6167, -4.5927],
[-163.0605, -68.4880, -2.7927],
[-162.2090, -70.2977, -2.7927],
[-162.2090, -70.2977, -4.5927],
[-163.0605, -68.4880, -4.5927],
],
device=device,
dtype=torch.float32,
)
box16b = torch.tensor(
[
[-167.5847, -70.6167, -2.7927],
[-166.7333, -72.4264, -2.7927],
[-166.7333, -72.4264, -4.5927],
[-167.5847, -70.6167, -4.5927],
[-163.0605, -68.4880, -2.7927],
[-162.2090, -70.2977, -2.7927],
[-162.2090, -70.2977, -4.5927],
[-163.0605, -68.4880, -4.5927],
],
device=device,
dtype=torch.float32,
)
vol, iou = overlap_fn(box16a[None], box16b[None])
self.assertClose(
iou, torch.tensor([[1.0]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# symmetry
vol, iou = overlap_fn(box16b[None], box16a[None])
self.assertClose(
iou, torch.tensor([[1.0]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# 17th test: From GH issue 1287
box17a = torch.tensor(
[
[-33.94158, -4.51639, 0.96941],
[-34.67156, -2.65437, 0.96941],
[-34.67156, -2.65437, -0.95367],
[-33.94158, -4.51639, -0.95367],
[-38.75954, -6.40521, 0.96941],
[-39.48952, -4.54319, 0.96941],
[-39.48952, -4.54319, -0.95367],
[-38.75954, -6.40521, -0.95367],
],
device=device,
dtype=torch.float32,
)
box17b = torch.tensor(
[
[-33.94159, -4.51638, 0.96939],
[-34.67158, -2.65437, 0.96939],
[-34.67158, -2.65437, -0.95368],
[-33.94159, -4.51638, -0.95368],
[-38.75954, -6.40523, 0.96939],
[-39.48953, -4.54321, 0.96939],
[-39.48953, -4.54321, -0.95368],
[-38.75954, -6.40523, -0.95368],
],
device=device,
dtype=torch.float32,
)
vol, iou = overlap_fn(box17a[None], box17b[None])
self.assertClose(
iou, torch.tensor([[1.0]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# symmetry
vol, iou = overlap_fn(box17b[None], box17a[None])
self.assertClose(
iou, torch.tensor([[1.0]], device=vol.device, dtype=vol.dtype), atol=1e-2
)
# 18th test: From GH issue 1287
box18a = torch.tensor(
[
[-105.6248, -32.7026, -1.2279],
[-106.4690, -30.8895, -1.2279],
[-106.4690, -30.8895, -3.0279],
[-105.6248, -32.7026, -3.0279],
[-110.1575, -34.8132, -1.2279],
[-111.0017, -33.0001, -1.2279],
[-111.0017, -33.0001, -3.0279],
[-110.1575, -34.8132, -3.0279],
],
device=device,
dtype=torch.float32,
)
box18b = torch.tensor(
[
[-105.5094, -32.9504, -1.0641],
[-106.4272, -30.9793, -1.0641],
[-106.4272, -30.9793, -3.1916],
[-105.5094, -32.9504, -3.1916],
[-110.0421, -35.0609, -1.0641],
[-110.9599, -33.0899, -1.0641],
[-110.9599, -33.0899, -3.1916],
[-110.0421, -35.0609, -3.1916],
],
device=device,
dtype=torch.float32,
)
# from Meshlab
vol_inters = 17.108501
vol_box1 = 18.000067
vol_box2 = 23.128527
iou_mesh = vol_inters / (vol_box1 + vol_box2 - vol_inters)
vol, iou = overlap_fn(box18a[None], box18b[None])
self.assertClose(
iou,
torch.tensor([[iou_mesh]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
self.assertClose(
vol,
torch.tensor([[vol_inters]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
# symmetry
vol, iou = overlap_fn(box18b[None], box18a[None])
self.assertClose(
iou,
torch.tensor([[iou_mesh]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
self.assertClose(
vol,
torch.tensor([[vol_inters]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
# 19th example: From GH issue 1287
box19a = torch.tensor(
[
[-59.4785, -15.6003, 0.4398],
[-60.2263, -13.6928, 0.4398],
[-60.2263, -13.6928, -1.3909],
[-59.4785, -15.6003, -1.3909],
[-64.1743, -17.4412, 0.4398],
[-64.9221, -15.5337, 0.4398],
[-64.9221, -15.5337, -1.3909],
[-64.1743, -17.4412, -1.3909],
],
device=device,
dtype=torch.float32,
)
box19b = torch.tensor(
[
[-59.4874, -15.5775, -0.1512],
[-60.2174, -13.7155, -0.1512],
[-60.2174, -13.7155, -1.9820],
[-59.4874, -15.5775, -1.9820],
[-64.1832, -17.4185, -0.1512],
[-64.9132, -15.5564, -0.1512],
[-64.9132, -15.5564, -1.9820],
[-64.1832, -17.4185, -1.9820],
],
device=device,
dtype=torch.float32,
)
# from Meshlab
vol_inters = 12.505723
vol_box1 = 18.918238
vol_box2 = 18.468531
iou_mesh = vol_inters / (vol_box1 + vol_box2 - vol_inters)
vol, iou = overlap_fn(box19a[None], box19b[None])
self.assertClose(
iou,
torch.tensor([[iou_mesh]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
self.assertClose(
vol,
torch.tensor([[vol_inters]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
# symmetry
vol, iou = overlap_fn(box19b[None], box19a[None])
self.assertClose(
iou,
torch.tensor([[iou_mesh]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
self.assertClose(
vol,
torch.tensor([[vol_inters]], device=vol.device, dtype=vol.dtype),
atol=1e-2,
)
def _test_real_boxes(self, overlap_fn, device):
data_filename = "./real_boxes.pkl"
@ -715,7 +917,86 @@ def get_plane_verts(box: torch.Tensor) -> torch.Tensor:
return plane_verts
def box_planar_dir(box: torch.Tensor, eps: float = 1e-4) -> torch.Tensor:
def get_tri_center_normal(tris: torch.Tensor) -> torch.Tensor:
"""
Returns the center and normal of triangles
Args:
tris: tensor of shape (T, 3, 3)
Returns:
center: tensor of shape (T, 3)
normal: tensor of shape (T, 3)
"""
add_dim0 = False
if tris.ndim == 2:
tris = tris.unsqueeze(0)
add_dim0 = True
ctr = tris.mean(1) # (T, 3)
normals = torch.zeros_like(ctr)
v0, v1, v2 = tris.unbind(1) # 3 x (T, 3)
# unvectorized solution
T = tris.shape[0]
for t in range(T):
ns = torch.zeros((3, 3), device=tris.device)
ns[0] = torch.cross(v0[t] - ctr[t], v1[t] - ctr[t], dim=-1)
ns[1] = torch.cross(v0[t] - ctr[t], v2[t] - ctr[t], dim=-1)
ns[2] = torch.cross(v1[t] - ctr[t], v2[t] - ctr[t], dim=-1)
i = torch.norm(ns, dim=-1).argmax()
normals[t] = ns[i]
if add_dim0:
ctr = ctr[0]
normals = normals[0]
normals = F.normalize(normals, dim=-1)
return ctr, normals
def get_plane_center_normal(planes: torch.Tensor) -> torch.Tensor:
"""
Returns the center and normal of planes
Args:
planes: tensor of shape (P, 4, 3)
Returns:
center: tensor of shape (P, 3)
normal: tensor of shape (P, 3)
"""
add_dim0 = False
if planes.ndim == 2:
planes = planes.unsqueeze(0)
add_dim0 = True
ctr = planes.mean(1) # (P, 3)
normals = torch.zeros_like(ctr)
v0, v1, v2, v3 = planes.unbind(1) # 4 x (P, 3)
# unvectorized solution
P = planes.shape[0]
for t in range(P):
ns = torch.zeros((6, 3), device=planes.device)
ns[0] = torch.cross(v0[t] - ctr[t], v1[t] - ctr[t], dim=-1)
ns[1] = torch.cross(v0[t] - ctr[t], v2[t] - ctr[t], dim=-1)
ns[2] = torch.cross(v0[t] - ctr[t], v3[t] - ctr[t], dim=-1)
ns[3] = torch.cross(v1[t] - ctr[t], v2[t] - ctr[t], dim=-1)
ns[4] = torch.cross(v1[t] - ctr[t], v3[t] - ctr[t], dim=-1)
ns[5] = torch.cross(v2[t] - ctr[t], v3[t] - ctr[t], dim=-1)
i = torch.norm(ns, dim=-1).argmax()
normals[t] = ns[i]
if add_dim0:
ctr = ctr[0]
normals = normals[0]
normals = F.normalize(normals, dim=-1)
return ctr, normals
def box_planar_dir(
box: torch.Tensor, dot_eps: float = DOT_EPS, area_eps: float = AREA_EPS
) -> torch.Tensor:
"""
Finds the unit vector n which is perpendicular to each plane in the box
and points towards the inside of the box.
@ -731,33 +1012,33 @@ def box_planar_dir(box: torch.Tensor, eps: float = 1e-4) -> torch.Tensor:
assert box.shape[0] == 8 and box.shape[1] == 3
# center point of each box
ctr = box.mean(0).view(1, 3)
box_ctr = box.mean(0).view(1, 3)
verts = get_plane_verts(box) # (6, 4, 3)
v0, v1, v2, v3 = verts.unbind(1) # each v of shape (6, 3)
# We project the ctr on the plane defined by (v0, v1, v2, v3)
# We define e0 to be the edge connecting (v1, v0)
# We define e1 to be the edge connecting (v2, v0)
# And n is the cross product of e0, e1, either pointing "inside" or not.
e0 = F.normalize(v1 - v0, dim=-1)
e1 = F.normalize(v2 - v0, dim=-1)
n = F.normalize(torch.cross(e0, e1, dim=-1), dim=-1)
# box planes
plane_verts = get_plane_verts(box) # (6, 4, 3)
v0, v1, v2, v3 = plane_verts.unbind(1)
plane_ctr, n = get_plane_center_normal(plane_verts)
# Check all verts are coplanar
if not ((v3 - v0).unsqueeze(1).bmm(n.unsqueeze(2)).abs() < eps).all().item():
if (
not (
F.normalize(v3 - v0, dim=-1).unsqueeze(1).bmm(n.unsqueeze(2)).abs()
< dot_eps
)
.all()
.item()
):
msg = "Plane vertices are not coplanar"
raise ValueError(msg)
# Check all faces have non zero area
area1 = torch.cross(v1 - v0, v2 - v0, dim=-1).norm(dim=-1) / 2
area2 = torch.cross(v3 - v0, v2 - v0, dim=-1).norm(dim=-1) / 2
if (area1 < eps).any().item() or (area2 < eps).any().item():
if (area1 < area_eps).any().item() or (area2 < area_eps).any().item():
msg = "Planes have zero areas"
raise ValueError(msg)
# We can write: `ctr = v0 + a * e0 + b * e1 + c * n`, (1).
# We can write: `box_ctr = plane_ctr + a * e0 + b * e1 + c * n`, (1).
# With <e0, n> = 0 and <e1, n> = 0, where <.,.> refers to the dot product,
# since that e0 is orthogonal to n. Same for e1.
"""
@ -768,16 +1049,17 @@ def box_planar_dir(box: torch.Tensor, eps: float = 1e-4) -> torch.Tensor:
B = torch.ones((numF, 2), dtype=torch.float32, device=device)
A[:, 0, 1] = (e0 * e1).sum(-1)
A[:, 1, 0] = (e0 * e1).sum(-1)
B[:, 0] = ((ctr - v0) * e0).sum(-1)
B[:, 1] = ((ctr - v1) * e1).sum(-1)
B[:, 0] = ((box_ctr - plane_ctr) * e0).sum(-1)
B[:, 1] = ((box_ctr - plane_ctr) * e1).sum(-1)
ab = torch.linalg.solve(A, B) # (numF, 2)
a, b = ab.unbind(1)
# solving for c
c = ((ctr - v0 - a.view(numF, 1) * e0 - b.view(numF, 1) * e1) * n).sum(-1)
c = ((box_ctr - plane_ctr - a.view(numF, 1) * e0 - b.view(numF, 1) * e1) * n).sum(-1)
"""
# Since we know that <e0, n> = 0 and <e1, n> = 0 (e0 and e1 are orthogonal to n),
# the above solution is equivalent to
c = ((ctr - v0) * n).sum(-1)
direc = F.normalize(box_ctr - plane_ctr, dim=-1) # (6, 3)
c = (direc * n).sum(-1)
# If c is negative, then we revert the direction of n such that n points "inside"
negc = c < 0.0
n[negc] *= -1.0
@ -848,7 +1130,7 @@ def box_volume(box: torch.Tensor) -> torch.Tensor:
return vols
def coplanar_tri_faces(tri1: torch.Tensor, tri2: torch.Tensor, eps: float = EPS):
def coplanar_tri_faces(tri1: torch.Tensor, tri2: torch.Tensor, eps: float = DOT_EPS):
"""
Determines whether two triangle faces in 3D are coplanar
Args:
@ -857,17 +1139,49 @@ def coplanar_tri_faces(tri1: torch.Tensor, tri2: torch.Tensor, eps: float = EPS)
Returns:
is_coplanar: bool
"""
v0, v1, v2 = tri1.unbind(0)
e0 = F.normalize(v1 - v0, dim=0)
e1 = F.normalize(v2 - v0, dim=0)
e2 = F.normalize(torch.cross(e0, e1), dim=0)
tri1_ctr, tri1_n = get_tri_center_normal(tri1)
tri2_ctr, tri2_n = get_tri_center_normal(tri2)
coplanar2 = torch.zeros((3,), dtype=torch.bool, device=tri1.device)
for i in range(3):
if (tri2[i] - v0).dot(e2).abs() < eps:
coplanar2[i] = 1
coplanar2 = coplanar2.all()
return coplanar2
check1 = tri1_n.dot(tri2_n).abs() > 1 - eps # checks if parallel
dist12 = torch.norm(tri1.unsqueeze(1) - tri2.unsqueeze(0), dim=-1)
dist12_argmax = dist12.argmax()
i1 = dist12_argmax // 3
i2 = dist12_argmax % 3
assert dist12[i1, i2] == dist12.max()
check2 = (
F.normalize(tri1[i1] - tri2[i2], dim=0).dot(tri1_n).abs() < eps
) or F.normalize(tri1[i1] - tri2[i2], dim=0).dot(tri2_n).abs() < eps
return check1 and check2
def coplanar_tri_plane(
tri: torch.Tensor, plane: torch.Tensor, n: torch.Tensor, eps: float = DOT_EPS
):
"""
Determines whether two triangle faces in 3D are coplanar
Args:
tri: tensor of shape (3, 3) of the vertices of the triangle
plane: tensor of shape (4, 3) of the vertices of the plane
n: tensor of shape (3,) of the unit "inside" direction on the plane
Returns:
is_coplanar: bool
"""
tri_ctr, tri_n = get_tri_center_normal(tri)
check1 = tri_n.dot(n).abs() > 1 - eps # checks if parallel
dist12 = torch.norm(tri.unsqueeze(1) - plane.unsqueeze(0), dim=-1)
dist12_argmax = dist12.argmax()
i1 = dist12_argmax // 4
i2 = dist12_argmax % 4
assert dist12[i1, i2] == dist12.max()
check2 = F.normalize(tri[i1] - plane[i2], dim=0).dot(n).abs() < eps
return check1 and check2
def is_inside(
@ -875,7 +1189,6 @@ def is_inside(
n: torch.Tensor,
points: torch.Tensor,
return_proj: bool = True,
eps: float = EPS,
):
"""
Computes whether point is "inside" the plane.
@ -900,12 +1213,13 @@ def is_inside(
p_proj: tensor of shape (P, 3) of the projected point on plane
"""
device = plane.device
v0, v1, v2, v3 = plane
e0 = F.normalize(v1 - v0, dim=0)
e1 = F.normalize(v2 - v0, dim=0)
if not torch.allclose(e0.dot(n), torch.zeros((1,), device=device), atol=1e-6):
v0, v1, v2, v3 = plane.unbind(0)
plane_ctr = plane.mean(0)
e0 = F.normalize(v0 - plane_ctr, dim=0)
e1 = F.normalize(v1 - plane_ctr, dim=0)
if not torch.allclose(e0.dot(n), torch.zeros((1,), device=device), atol=1e-2):
raise ValueError("Input n is not perpendicular to the plane")
if not torch.allclose(e1.dot(n), torch.zeros((1,), device=device), atol=1e-6):
if not torch.allclose(e1.dot(n), torch.zeros((1,), device=device), atol=1e-2):
raise ValueError("Input n is not perpendicular to the plane")
add_dim = False
@ -914,7 +1228,7 @@ def is_inside(
add_dim = True
assert points.shape[1] == 3
# Every point p can be written as p = v0 + a e0 + b e1 + c n
# Every point p can be written as p = ctr + a e0 + b e1 + c n
# If return_proj is True, we need to solve for (a, b)
p_proj = None
@ -924,16 +1238,17 @@ def is_inside(
[[1.0, e0.dot(e1)], [e0.dot(e1), 1.0]], dtype=torch.float32, device=device
)
B = torch.zeros((2, points.shape[0]), dtype=torch.float32, device=device)
B[0, :] = torch.sum((points - v0.view(1, 3)) * e0.view(1, 3), dim=-1)
B[1, :] = torch.sum((points - v0.view(1, 3)) * e1.view(1, 3), dim=-1)
B[0, :] = torch.sum((points - plane_ctr.view(1, 3)) * e0.view(1, 3), dim=-1)
B[1, :] = torch.sum((points - plane_ctr.view(1, 3)) * e1.view(1, 3), dim=-1)
ab = A.inverse() @ B # (2, P)
p_proj = v0.view(1, 3) + ab.transpose(0, 1) @ torch.stack((e0, e1), dim=0)
p_proj = plane_ctr.view(1, 3) + ab.transpose(0, 1) @ torch.stack(
(e0, e1), dim=0
)
# solving for c
# c = (point - v0 - a * e0 - b * e1).dot(n)
c = torch.sum((points - v0.view(1, 3)) * n.view(1, 3), dim=-1)
ins = c > -eps
# c = (point - ctr - a * e0 - b * e1).dot(n)
direc = torch.sum((points - plane_ctr.view(1, 3)) * n.view(1, 3), dim=-1)
ins = direc >= 0.0
if add_dim:
assert p_proj.shape[0] == 1
@ -942,7 +1257,7 @@ def is_inside(
return ins, p_proj
def plane_edge_point_of_intersection(plane, n, p0, p1):
def plane_edge_point_of_intersection(plane, n, p0, p1, eps: float = DOT_EPS):
"""
Finds the point of intersection between a box plane and
a line segment connecting (p0, p1).
@ -961,11 +1276,17 @@ def plane_edge_point_of_intersection(plane, n, p0, p1):
# The point of intersection can be parametrized
# p = p0 + a (p1 - p0) where a in [0, 1]
# We want to find a such that p is on plane
# <p - v0, n> = 0
v0, v1, v2, v3 = plane
a = -(p0 - v0).dot(n) / (p1 - p0).dot(n)
p = p0 + a * (p1 - p0)
return p, a
# <p - ctr, n> = 0
# if segment (p0, p1) is parallel to plane (it can only be on it)
direc = F.normalize(p1 - p0, dim=0)
if direc.dot(n).abs() < eps:
return (p1 + p0) / 2.0, 0.5
else:
ctr = plane.mean(0)
a = -(p0 - ctr).dot(n) / ((p1 - p0).dot(n))
p = p0 + a * (p1 - p0)
return p, a
"""
@ -983,7 +1304,6 @@ def clip_tri_by_plane_oneout(
vout: torch.Tensor,
vin1: torch.Tensor,
vin2: torch.Tensor,
eps: float = EPS,
) -> Tuple[torch.Tensor, torch.Tensor]:
"""
Case (a).
@ -1004,10 +1324,10 @@ def clip_tri_by_plane_oneout(
device = plane.device
# point of intersection between plane and (vin1, vout)
pint1, a1 = plane_edge_point_of_intersection(plane, n, vin1, vout)
assert a1 >= -eps and a1 <= 1.0 + eps, a1
assert a1 >= -0.0001 and a1 <= 1.0001, a1
# point of intersection between plane and (vin2, vout)
pint2, a2 = plane_edge_point_of_intersection(plane, n, vin2, vout)
assert a2 >= -eps and a2 <= 1.0 + eps, a2
assert a2 >= -0.0001 and a2 <= 1.0001, a2
verts = torch.stack((vin1, pint1, pint2, vin2), dim=0) # 4x3
faces = torch.tensor(
@ -1022,7 +1342,6 @@ def clip_tri_by_plane_twoout(
vout1: torch.Tensor,
vout2: torch.Tensor,
vin: torch.Tensor,
eps: float = EPS,
) -> Tuple[torch.Tensor, torch.Tensor]:
"""
Case (b).
@ -1042,10 +1361,10 @@ def clip_tri_by_plane_twoout(
device = plane.device
# point of intersection between plane and (vin, vout1)
pint1, a1 = plane_edge_point_of_intersection(plane, n, vin, vout1)
assert a1 >= -eps and a1 <= 1.0 + eps, a1
assert a1 >= -0.0001 and a1 <= 1.0001, a1
# point of intersection between plane and (vin, vout2)
pint2, a2 = plane_edge_point_of_intersection(plane, n, vin, vout2)
assert a2 >= -eps and a2 <= 1.0 + eps, a2
assert a2 >= -0.0001 and a2 <= 1.0001, a2
verts = torch.stack((vin, pint1, pint2), dim=0) # 3x3
faces = torch.tensor(
@ -1071,6 +1390,9 @@ def clip_tri_by_plane(plane, n, tri_verts) -> Union[List, torch.Tensor]:
tri_verts: tensor of shape (K, 3, 3) of the vertex coordinates of the triangles formed
after clipping. All K triangles are now "inside" the plane.
"""
if coplanar_tri_plane(tri_verts, plane, n):
return tri_verts.view(1, 3, 3)
v0, v1, v2 = tri_verts.unbind(0)
isin0, _ = is_inside(plane, n, v0)
isin1, _ = is_inside(plane, n, v1)
@ -1191,7 +1513,7 @@ def box3d_overlap_naive(box1: torch.Tensor, box2: torch.Tensor):
for i2 in range(tri_verts2.shape[0]):
if (
coplanar_tri_faces(tri_verts1[i1], tri_verts2[i2])
and tri_verts_area(tri_verts1[i1]) > 1e-4
and tri_verts_area(tri_verts1[i1]) > AREA_EPS
):
keep2[i2] = 0
keep2 = keep2.nonzero()[:, 0]