Skip to content

Commit

Permalink
structured point clouds: implementation (wip)
Browse files Browse the repository at this point in the history
  • Loading branch information
@stefanmaierhofer
stefanmaierhofer committed Oct 5, 2023
1 parent 8ef3e80 commit 3e9b502
Show file tree
Hide file tree
Showing 4 changed files with 142 additions and 190 deletions.
96 changes: 41 additions & 55 deletions src/Aardvark.Geometry.PointSet/Octrees/Merge.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,10 +115,9 @@ public static (IPointCloudNode, bool) CollapseLeafNodes(this IPointCloudNode sel
nsla.Count > 0 ? nsla : null,
jsla.Count > 0 ? jsla : null,
ksla.Count > 0 ? ksla : null,
partIndices: null, // TODO
qsla,
bbox: null
);
throw new NotImplementedException("PARTINDICES");

if (config.NormalizePointDensityGlobal)
{
Expand Down Expand Up @@ -164,10 +163,10 @@ public static (IPointCloudNode, bool) CollapseLeafNodes(this IPointCloudNode sel
return (self.WriteToStore(), true);
}
}

/// <summary>
/// If cell is a leaf, it will be split once (non-recursive, without taking into account any split limit).
/// If cell is not a leaf, this is an invalid operation.
/// If node is a leaf, it will be split once (non-recursive, without taking into account any split limit).
/// If node is not a leaf, this is an invalid operation.
/// </summary>
public static IPointCloudNode ForceSplitLeaf(this IPointCloudNode self, ImportConfig config)
{
Expand All @@ -180,74 +179,61 @@ public static IPointCloudNode ForceSplitLeaf(this IPointCloudNode self, ImportCo
if (self.PointCountCell == 0) throw new InvalidOperationException();
if (self.PointCountTree != self.PointCountCell) throw new InvalidOperationException();

var subnodesPoints = new List<V3d>[8];
var subnodesColors = self.HasColors ? new List<C4b>[8] : null;
var subnodesNormals = self.HasNormals ? new List<V3f>[8] : null;
var subnodesIntensities = self.HasIntensities ? new List<int>[8] : null;
var subnodesClassifications = self.HasClassifications ? new List<byte>[8] : null;
object?[]? subnodesPartIndices = self.HasPartIndices ? throw new NotImplementedException("PARTINDICES") : null;

var pa = self.PositionsAbsolute;
var ca = self.Colors?.Value;
var na = self.Normals?.Value;
var ia = self.Intensities?.Value;
var ka = self.Classifications?.Value;
var ps = self.PositionsAbsolute;
var cs = self.Colors?.Value;
var ns = self.Normals?.Value;
var js = self.Intensities?.Value;
var ks = self.Classifications?.Value;
var qs = self.PartIndices;

var pss = new V3d[]?[8];
var css = self.HasColors ? new C4b[]?[8] : null;
var nss = self.HasNormals ? new V3f[]?[8] : null;
var jss = self.HasIntensities ? new int[]?[8] : null;
var kss = self.HasClassifications ? new byte[]?[8] : null;
var qss = self.HasPartIndices ? new object?[8] : null;

var imax = self.PointCountCell;
if (pa.Length != imax) throw new InvalidOperationException();
if (ps.Length != imax) throw new InvalidOperationException();

for (var i = 0; i < imax; i++)
var ias = new List<int>[8];
for (var i = 0; i < 8; i++) ias[i] = new();
for (var i = 0; i < imax; i++) ias[self.GetSubIndex(ps[i])].Add(i);

for (var i = 0; i < 8; i++)
{
var si = self.GetSubIndex(pa[i]);
if (subnodesPoints[si] == null)
{
subnodesPoints[si] = new List<V3d>();
if (subnodesColors != null) subnodesColors[si] = new List<C4b>();
if (subnodesNormals != null) subnodesNormals[si] = new List<V3f>();
if (subnodesIntensities != null) subnodesIntensities[si] = new List<int>();
if (subnodesClassifications != null) subnodesClassifications[si] = new List<byte>();
}
subnodesPoints[si].Add(pa[i]);
subnodesColors?[si].Add(ca![i]);
subnodesNormals?[si].Add(na![i]);
subnodesIntensities?[si].Add(ia![i]);
subnodesClassifications?[si].Add(ka![i]);
var ia = ias[i];
if (ia.Count == 0) continue;

pss[i] = ps.Subset(ia);
if (css != null) css[i] = cs?.Subset(ia);
if (nss != null) nss[i] = ns?.Subset(ia);
if (jss != null) jss[i] = js?.Subset(ia);
if (kss != null) kss[i] = ks?.Subset(ia);
if (qss != null) qss[i] = PartIndexUtils.Subset(qs, ia);
}

var subnodes = new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
if (subnodesPoints[i] == null) continue;
var subPs = pss[i];
if (subPs == null) continue;

var subCell = self.Cell.GetOctant(i);
if (!self.Cell.Contains(subCell)) throw new InvalidOperationException();
if (self.Cell.Exponent != subCell.Exponent + 1) throw new InvalidOperationException();

var chunk = new Chunk(
subnodesPoints[i],
subnodesColors?[i],
subnodesNormals?[i],
subnodesIntensities?[i],
subnodesClassifications?[i],
partIndices: null, // TODO
subCell.BoundingBox
);
throw new NotImplementedException("PARTINDICES");
var chunk = new Chunk(subPs, css?[i], nss?[i], jss?[i], kss?[i], qss?[i], subCell.BoundingBox);

if (config.NormalizePointDensityGlobal)
{
chunk = chunk.ImmutableFilterMinDistByCell(subCell, config.ParseConfig);
}
var builder = InMemoryPointSet.Build(
subnodesPoints[i],
subnodesColors?[i],
subnodesNormals?[i],
subnodesIntensities?[i],
subnodesClassifications?[i],
subnodesPartIndices?[i],
subCell,
int.MaxValue
);

var builder = InMemoryPointSet.Build(subPs, css?[i], nss?[i], jss?[i], kss?[i], qss?[i], subCell, int.MaxValue);

var subnode = builder.ToPointSetNode(config.Storage, isTemporaryImportNode: true);
if (subnode.PointCountTree > subnodesPoints[i].Count) throw new InvalidOperationException();
if (subnode.PointCountTree > subPs.Length) throw new InvalidOperationException();
if (!self.Cell.Contains(subnode.Cell)) throw new InvalidOperationException();
if (self.Cell.Exponent != subnode.Cell.Exponent + 1) throw new InvalidOperationException();

Expand Down
181 changes: 86 additions & 95 deletions src/Aardvark.Geometry.PointSet/Queries/QueriesDirectedRay3d.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,112 +17,103 @@ You should have received a copy of the GNU Affero General Public License
using Aardvark.Base;
using Aardvark.Data.Points;

namespace Aardvark.Geometry.Points
namespace Aardvark.Geometry.Points;

/// <summary>
/// </summary>
public static partial class Queries
{
/// <summary>
/// Enumerates Points within given distance of a Ray3d whose Ts lie between tMin and tMax.
/// Chunks are approximately sorted along the ray direction.
/// </summary>
public static partial class Queries
public static IEnumerable<Chunk> QueryPointsNearRay(
this PointSet ps,
Ray3d ray,
double maxDistanceToRay,
double tMin,
double tMax,
int minCellExponent = int.MinValue
)
{
/// <summary>
/// Enumerates Points within given distance of a Ray3d whose Ts lie between tMin and tMax. Chunks are approximately sorted along the ray direction.
/// </summary>
public static IEnumerable<Chunk> QueryPointsNearRay(
this PointSet ps,
Ray3d ray,
double maxDistanceToRay,
double tMin,
double tMax,
int minCellExponent = int.MinValue
)
{
if(ps.Root == null) return Enumerable.Empty<Chunk>();
return ps.Root.Value.QueryPointsNearRay(ray,maxDistanceToRay,tMin,tMax,minCellExponent);
}
if(ps.Root == null) return Enumerable.Empty<Chunk>();
return ps.Root.Value.QueryPointsNearRay(ray,maxDistanceToRay,tMin,tMax,minCellExponent);
}

/// <summary>
/// Enumerates Points within given distance of a Ray3d whose Ts lie between tMin and tMax. Chunks are approximately sorted along the ray direction.
/// </summary>
public static IEnumerable<Chunk> QueryPointsNearRay(
this IPointCloudNode node,
Ray3d ray,
double maxDistanceToRay,
double tMin,
double tMax,
int minCellExponent = int.MinValue
)
{
var fastRay = new FastRay3d(ray);
/// <summary>
/// Enumerates Points within given distance of a Ray3d whose Ts lie between tMin and tMax.
/// Chunks are approximately sorted along the ray direction.
/// </summary>
public static IEnumerable<Chunk> QueryPointsNearRay(
this IPointCloudNode node,
Ray3d ray,
double maxDistanceToRay,
double tMin,
double tMax,
int minCellExponent = int.MinValue
)
{
var fastRay = new FastRay3d(ray);

double t0 = System.Double.NegativeInfinity;
double t1 = System.Double.PositiveInfinity;
double t0 = double.NegativeInfinity;
double t1 = double.PositiveInfinity;

var box =
Box3d.FromCenterAndSize(
node.BoundingBoxExactGlobal.Center,
node.BoundingBoxExactGlobal.Size + V3d.One * maxDistanceToRay
);
if(fastRay.Intersects(box, ref t0, ref t1)) {
if(t1 < tMin || t0 > tMax) { yield break; }
var box = Box3d.FromCenterAndSize(
node.BoundingBoxExactGlobal.Center,
node.BoundingBoxExactGlobal.Size + V3d.One * maxDistanceToRay
);

if (node.IsLeaf || node.Cell.Exponent == minCellExponent) {
var qs = node.Positions.Value;
if (fastRay.Intersects(box, ref t0, ref t1))
{
if(t1 < tMin || t0 > tMax) { yield break; }

var ps = default(List<V3d>);
var cs = default(List<C4b>);
var ns = default(List<V3f>);
var js = default(List<int>);
var ks = default(List<byte>);
if (node.IsLeaf || node.Cell.Exponent == minCellExponent)
{
var qs = node.Positions.Value;

for (var i = 0; i < qs.Length; i++)
{
var pWorld = (V3d)qs[i] + node.Center;
var d = ray.GetMinimalDistanceTo(pWorld);
var tp = ray.GetTOfProjectedPoint(pWorld);
if (d > maxDistanceToRay || tp < tMin || tp > tMax) continue;
if (ps == null) Init();
var ps = new List<V3d>();
var ia = new List<int>();

ps!.Add(pWorld);
if (node.HasColors) cs!.Add(node.Colors!.Value[i]);
if (node.HasNormals) ns!.Add(node.Normals!.Value[i]);
if (node.HasIntensities) js!.Add(node.Intensities!.Value[i]);
if (node.HasClassifications) ks!.Add(node.Classifications.Value[i]);
}
for (var i = 0; i < qs.Length; i++)
{
var pWorld = (V3d)qs[i] + node.Center;
var d = ray.GetMinimalDistanceTo(pWorld);
var tp = ray.GetTOfProjectedPoint(pWorld);
if (d > maxDistanceToRay || tp < tMin || tp > tMax) continue;

if (ps != null)
{
throw new NotImplementedException("PARTINDICES 13912850-d9fd-4aba-819c-8eb3b6cbc2dc");
yield return new Chunk(ps, cs, ns, js, ks, partIndices: null /* TODO */, bbox: null);
}
ps.Add(pWorld);
ia.Add(i);
}

if (ia.Count > 0) yield return new Chunk(
ps,
node.Colors?.Value.Subset(ia),
node.Normals?.Value.Subset(ia),
node.Intensities?.Value.Subset(ia),
node.Classifications?.Value.Subset(ia),
node.PartIndices?.Subset(ia),
bbox: null
);
}
else
{
var sorted = node.Subnodes.OrderBy(
c => c == null ? double.PositiveInfinity : Vec.Distance(new V3d(c.Value.BoundingBoxExactGlobal.Center), ray.Origin)
);

void Init()
{
ps = new List<V3d>();
cs = node.HasColors ? new List<C4b>() : null;
ns = node.HasNormals ? new List<V3f>() : null;
js = node.HasIntensities ? new List<int>() : null;
ks = node.HasClassifications ? new List<byte>() : null;
}
} else {
var sorted =
node.Subnodes.OrderBy(c =>
(c==null)?System.Double.PositiveInfinity:
Vec.Distance(
new V3d(c.Value.BoundingBoxExactGlobal.Center),
ray.Origin
)
);
foreach(var c in sorted) {
if(c==null) continue;
if (t0 > tMin) { tMin = t0; }
if (t1 < tMax) { tMax = t1; }
var ress = c.Value.QueryPointsNearRay(ray, maxDistanceToRay, tMin, tMax, minCellExponent);
foreach(var res in ress) yield return res;
}
} // node is leafish
foreach(var c in sorted)
{
if (c == null) continue;
if (t0 > tMin) tMin = t0;
if (t1 < tMax) tMax = t1;
var ress = c.Value.QueryPointsNearRay(ray, maxDistanceToRay, tMin, tMax, minCellExponent);
foreach(var res in ress) yield return res;
}
}
else {
yield break;
} // ray intersects bounds
} // QueryPointsNearRay
} // static partial class
} // namespace
}
else
{
yield break;
}
}
}
30 changes: 7 additions & 23 deletions src/Aardvark.Geometry.PointSet/Queries/QueriesGeneric.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,35 +55,19 @@ public static IEnumerable<Chunk> QueryPoints(this IPointCloudNode node,
}
else // partially inside
{
var psRaw = node.PositionsAbsolute;
var ps = node.PositionsAbsolute;
var csRaw = node.HasColors ? node.Colors.Value : null;
var nsRaw = node.HasNormals ? node.Normals.Value : null;
var jsRaw = node.HasIntensities ? node.Intensities.Value : null;
var ksRaw = node.HasClassifications ? node.Classifications!.Value : null;
var qsRaw = node.PartIndices;

var ps = new List<V3d>();
var cs = csRaw != null ? new List<C4b>() : null;
var ns = nsRaw != null ? new List<V3f>() : null;
var js = jsRaw != null ? new List<int>() : null;
var ks = ksRaw != null ? new List<byte>() : null;
var ia = new List<int>();
for (var i = 0; i < ps.Length; i++) if (isPositionInside(ps[i])) ia.Add(i);

for (var i = 0; i < psRaw.Length; i++)
{
var p = psRaw[i];
if (isPositionInside(p))
{
ps.Add(p);
if (csRaw != null) cs!.Add(csRaw[i]);
if (nsRaw != null) ns!.Add(nsRaw[i]);
if (jsRaw != null) js!.Add(jsRaw[i]);
if (ksRaw != null) ks!.Add(ksRaw[i]);
}
}
if (ps.Count > 0)
{
throw new NotImplementedException("PARTINDICES 7335ac35-e5e1-4e90-8907-48ebb9b66b1b");
yield return new Chunk(ps, cs, ns, js, ks, partIndices: null /* TODO */, bbox: null);
}
if (ia.Count > 0) yield return new Chunk(
ps.Subset(ia), csRaw?.Subset(ia), nsRaw?.Subset(ia), jsRaw?.Subset(ia), ksRaw?.Subset(ia), qsRaw?.Subset(ia), bbox: null
);
}
}
else
Expand Down
Loading

0 comments on commit 3e9b502

Please sign in to comment.