/// <summary>
/// Returns index of a sub-cell covering a fragment of space that given point occupies.
/// </summary>
private int GetCellIndex(PointCloudPoint point)
{
var relativePosition = point.Position - alignedBounds.min;
var x = (int)(relativePosition.x / cellStep);
var y = (int)(relativePosition.y / cellStep);
var z = (int)(relativePosition.z / cellStep);
// Upper bounds are inclusive, so last set of cells in each axis must also accept points from there
if (x >= cellsPerAxis[0])
{
x = cellsPerAxis[0] - 1;
}
if (y >= cellsPerAxis[1])
{
y = cellsPerAxis[1] - 1;
}
if (z >= cellsPerAxis[2])
{
z = cellsPerAxis[2] - 1;
}
var flat = x + y * cellsPerAxis[0] + z * cellsPerAxis[0] * cellsPerAxis[1];
return(flat);
}
///<inheritdoc/>
public bool TryAddPoint(PointCloudPoint point, out PointCloudPoint?replacedPoint)
{
var cellIndex = GetCellIndex(point);
// Cell is already occupied, check distance to center
if (outputPoints.TryGetValue(cellIndex, out var current))
{
// New point is closer - place it here, pass old point to child
if (NewPointIsBetter(cellIndex, current.Position, point.Position))
{
outputPoints[cellIndex] = point;
replacedPoint = current;
return(true);
}
// Old point is closer - pass new point to child
else
{
replacedPoint = null;
return(false);
}
}
// Cell is empty - place new point there
else
{
outputPoints[cellIndex] = point;
replacedPoint = null;
return(true);
}
}
private bool classPoints(float[] bbox, out List <PointCloudPoint> points)
{
points = new List <PointCloudPoint>();
var ymax = cam.pixelHeight * (1 - bbox[0]);
var xmax = cam.pixelWidth * (1 - bbox[1]);
var ymin = cam.pixelHeight * (1 - bbox[2]);
var xmin = cam.pixelWidth * (1 - bbox[3]);
Vector3 centroid = new Vector3(0, 0, 0);
if (Frame.PointCloud.PointCount > 0)
{
for (var i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint pp = Frame.PointCloud.GetPointAsStruct(i);
centroid += pp;
Vector3 screenpoint = cam.WorldToScreenPoint(pp);
if (Rect.MinMaxRect(xmin, ymin, xmax, ymax).Contains(screenpoint) &&
Vector3.Distance(centroid / (i + 1), pp) < 0.5)
{
points.Add(pp);
}
}
}
return(points.Count > 0);
}
public OctreeNodeProcessor(string dataPath, TreeImportSettings settings, TreeImportData importData) : base(dataPath, settings, importData)
{
ChildBuffers = new PointCloudPoint[ChildCount][];
for (var i = 0; i < ChildCount; ++i)
{
ChildBuffers[i] = new PointCloudPoint[settings.chunkSize];
}
ChildCounts = new int[ChildCount];
ChildFileCounts = new int[ChildCount];
}
/// <summary>
/// <para>Registers a single point for further processing.</para>
/// <para>Point should use world-space coordinates.</para>
/// </summary>
public void AddPoint(PointCloudPoint point)
{
points[pointCount++] = point;
totalPointsCount++;
if (pointCount == Settings.chunkSize)
{
FlushTmpFile(points, 0, Settings.chunkSize);
pointCount = 0;
}
}
private void AddPointsToList()
{
for (int i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint point = Frame.PointCloud.GetPointAsStruct(i);
if (Vector3.Distance(point.Position, InnerCam.transform.position) < limitDistance && CamRaycast(point.Position))
{
PointClouds.Add(point.Position);
}
}
newPointCloudsNum = PointClouds.Count;
}
private void SavePointCloud(int key)
{
if (Frame.PointCloud.IsUpdatedThisFrame)
{
SaveData_CreateNewPointCloudFile(key);
for (int i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint point = Frame.PointCloud.GetPointAsStruct(i);
if (point.Id > 0)
{
SaveData_SavePoint(point.Position.x, point.Position.y, point.Position.z, point.Confidence, key);
}
}
}
}
public void Execute(int index)
{
var x = (X[index] + OutputCenterX) * OutputScaleX;
var y = (Y[index] + OutputCenterY) * OutputScaleY;
var z = (Z[index] + OutputCenterZ) * OutputScaleZ;
var pt = Transform.MultiplyVector(new Vector3((float)x, (float)y, (float)z));
Output[index] = new PointCloudPoint()
{
Position = pt,
Color = Color[index],
};
++Counts[ThreadIndex];
}
/// <inheritdoc/>
protected override void PassPointToChild(PointCloudPoint point)
{
var childIndex = TreeUtility.GetQuadtreeChildIndex(NodeRecord.Bounds, point.Position);
if (ChildFileCounts[childIndex] == 0 && ChildCounts[childIndex] == 0)
{
var childRecord = CreateChildRecord(childIndex);
ChildNodeRecords.Add(childRecord);
}
ChildBuffers[childIndex][ChildCounts[childIndex]++] = point;
if (ChildCounts[childIndex] == Settings.chunkSize)
{
FlushChildFile(childIndex);
}
}
public bool TryAddPoint(PointCloudPoint point, out PointCloudPoint?replacedPoint)
{
replacedPoint = null;
GetCellCoords(point, out var x, out var y, out var z);
for (var i = x - 1; i <= x + 1; ++i)
{
if (i < 0 || i >= cellsPerAxis[0])
{
continue;
}
for (var j = y - 1; j <= y + 1; ++j)
{
if (j < 0 || j >= cellsPerAxis[1])
{
continue;
}
for (var k = z - 1; k <= z + 1; ++k)
{
if (k < 0 || k >= cellsPerAxis[2])
{
continue;
}
if (PositionConflicts(point, i, j, k))
{
return(false);
}
}
}
}
var index = GetFlatIndex(x, y, z);
if (!outputPoints.ContainsKey(index))
{
outputPoints.Add(index, new List <PointCloudPoint>());
}
outputPoints[index].Add(point);
return(true);
}
// Update is called once per frame
void Update()
{
if (Frame.PointCloud.IsUpdatedThisFrame)
{
if (logger != null)
{
logger.text = "Have Points";
}
if (AddVoxels)
{
CameraImageBytes cim = Frame.CameraImage.AcquireCameraImageBytes();
for (int i = 0; i < Frame.PointCloud.PointCount; i++)
{
Color colour = new Color(0, 0, 0);
bool foundColour = false;
PointCloudPoint p = Frame.PointCloud.GetPointAsStruct(i);
Vector3 cameraCoordinates = arCamera.WorldToViewportPoint(p);
if (cim.IsAvailable)
{
var uvQuad = Frame.CameraImage.DisplayUvCoords;
int cx = (int)(cameraCoordinates.x * cim.Width);
int cy = (int)((1.0f - cameraCoordinates.y) * cim.Height);
colour = GetColourAt(cim, cx, cy, out foundColour);
}
if (foundColour)
{
tree.addPoint(p, colour);
}
}
cim.Release();
}
tree.renderOctTree(voxelParent);
}
else
{
if (logger != null)
{
logger.text = "No Points";
}
}
}
public int InitializePointsCheck(int originTrackInstantiatePoint)
{
PointsAngleA = true; PointsAngleB = true; PointsAngleC = true; //can go this direction
numberOfPoints = Frame.PointCloud.PointCount;
Debug.Log("jj_Num of Points: " + numberOfPoints);
if (numberOfPoints > 0) //check all points to see theie angle, height and distance
{
for (int i = 0; i < numberOfPoints; i++)
{
PointCloudPoint myPoint = Frame.PointCloud.GetPointAsStruct(i);
Vector3 Position = myPoint.Position;
CheckPoint(Position, i, originTrackInstantiatePoint);
}
}
//now choose a random option from available directions
List <int> PointList = new List <int>();
if (PointsAngleA == true)
{
PointList.Add(0);
}
if (PointsAngleB == true)
{
PointList.Add(1);
}
if (PointsAngleC == true)
{
PointList.Add(2);
}
if (PointList.Count == 0)
{
return(-1); //no free direction
}
else
{
int j = PointList[Random.Range(0, PointList.Count)];
Debug.Log("JJ_pointList: " + j);
PointList.Clear(); //clear list for next time around
return(j);
}
}
private bool PositionConflicts(PointCloudPoint point, int cellX, int cellY, int cellZ)
{
var index = GetFlatIndex(cellX, cellY, cellZ);
if (!outputPoints.ContainsKey(index))
{
return(false);
}
foreach (var p in outputPoints[index])
{
var squaredDistance = Vector3.SqrMagnitude(point.Position - p.Position);
if (squaredDistance < minDistanceSquared)
{
return(true);
}
}
return(false);
}
public PointCloudPoint[] ToArray()
{
var count = 0;
foreach (var outputPointGroup in outputPoints)
{
count += outputPointGroup.Value.Count;
}
var result = new PointCloudPoint[count];
var offset = 0;
foreach (var outputPointGroup in outputPoints)
{
foreach (var point in outputPointGroup.Value)
{
result[offset++] = point;
}
}
return(result);
}
public QuadtreeNodeProcessor(string dataPath, TreeImportSettings settings) : base(dataPath, settings)
{
ChildBuffers = new PointCloudPoint[ChildCount][];
for (var i = 0; i < ChildCount; ++i)
{
ChildBuffers[i] = new PointCloudPoint[settings.chunkSize];
}
ChildCounts = new int[ChildCount];
ChildFileCounts = new int[ChildCount];
if (settings.sampling == TreeImportSettings.SamplingMethod.CellCenter)
{
PointCollection = new CellCenterPointCollection();
}
else
{
PointCollection = new PoissonDiskPointCollection();
}
PointCollection.Initialize(settings);
}
private void GetCellCoords(PointCloudPoint point, out int x, out int y, out int z)
{
var relativePosition = point.Position - nodeRecord.Bounds.min;
x = (int)(relativePosition.x / cellStep.x);
y = (int)(relativePosition.y / cellStep.y);
z = (int)(relativePosition.z / cellStep.z);
if (x >= cellsPerAxis[0])
{
x = cellsPerAxis[0] - 1;
}
if (y >= cellsPerAxis[1])
{
y = cellsPerAxis[1] - 1;
}
if (z >= cellsPerAxis[2])
{
z = cellsPerAxis[2] - 1;
}
}
unsafe PointCloudPoint[] Convert(AssetImportContext context, byte *ptr, int stride, int count, List <PointElement> elements, PointCloudBounds bounds, out bool hasColor)
{
var name = Path.GetFileName(context.assetPath);
var counts = new NativeArray <int>(JobsUtility.MaxJobThreadCount, Allocator.TempJob);
try
{
var points = new PointCloudPoint[count];
fixed(PointCloudPoint *output = points)
{
double scaleX, scaleY, scaleZ;
double centerX, centerY, centerZ;
if (Normalize)
{
centerX = -0.5 * (bounds.MaxX + bounds.MinX);
centerY = -0.5 * (bounds.MaxY + bounds.MinY);
centerZ = -0.5 * (bounds.MaxZ + bounds.MinZ);
scaleX = 2.0 / (bounds.MaxX - bounds.MinX);
scaleY = 2.0 / (bounds.MaxY - bounds.MinY);
scaleZ = 2.0 / (bounds.MaxZ - bounds.MinZ);
}
else if (Center)
{
centerX = -0.5 * (bounds.MaxX + bounds.MinX);
centerY = -0.5 * (bounds.MaxY + bounds.MinY);
centerZ = -0.5 * (bounds.MaxZ + bounds.MinZ);
scaleX = scaleY = scaleZ = 1.0;
}
else
{
centerX = centerY = centerZ = 0.0;
scaleX = scaleY = scaleZ = 1.0;
}
var job = new PointCloudConvertJob()
{
X = GetInputAccess(PointElementName.X, elements, ptr, stride),
Y = GetInputAccess(PointElementName.Y, elements, ptr, stride),
Z = GetInputAccess(PointElementName.Z, elements, ptr, stride),
Color = GetColorAccess(context, elements, ptr, stride),
Output = output,
Transform = GetTransform(),
OutputCenterX = centerX,
OutputCenterY = centerY,
OutputCenterZ = centerZ,
OutputScaleX = scaleX,
OutputScaleY = scaleY,
OutputScaleZ = scaleZ,
Counts = (int *)counts.GetUnsafePtr(),
ThreadIndex = 0,
};
var h = job.Schedule((int)count, 65536);
while (!h.IsCompleted)
{
System.Threading.Thread.Sleep(100);
int processed = counts.Sum();
float progress = (float)((double)processed / count);
EditorUtility.DisplayProgressBar($"Importing {name}", $"{processed:N0} points", progress);
}
hasColor = job.Color.Color != null;
return(points);
}
}
finally
{
EditorUtility.ClearProgressBar();
counts.Dispose();
}
}
// Update is called once per frame
void Update()
{
if (connected)
{
string user = "******";
//var reply = client.GetCameraTransform(new CameraRequest { Message = user });
var update = new CameraUpdate
{
X = transform.position.x,
Y = transform.position.y,
Z = transform.position.z,
Q = transform.rotation.w,
W = transform.rotation.x,
R = transform.rotation.y,
T = transform.rotation.z
};
if (isCameraStreaming)
{
var reply = client.UpdateCamera(update);
}
if (Session.Status != SessionStatus.Tracking)
{
return;
}
if (Frame.PointCloud.PointCount > 0 && Frame.PointCloud.IsUpdatedThisFrame && readyForNextFrame && isCallComplete)
{
for (var i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint pcpoint = Frame.PointCloud.GetPointAsStruct(i);
Vector3 screenCoordinate = cam.WorldToScreenPoint(pcpoint.Position);
if (Vector3.Dot(transform.forward, pcpoint - cam.transform.position) >= 0)
{
// checking if screenPoint is inside the screen area:
if (Rect.MinMaxRect(0, 0, Screen.width, Screen.height).Contains(screenCoordinate))
{
Debug.Log("Cam Width" + cam.pixelWidth);
int x_index = (int)screenCoordinate.x * rgb_width / cam.pixelWidth;
int y_index = (int)screenCoordinate.y * rgb_height / cam.pixelHeight;
//BELOW IMPLEMENTATION DOES NOT WORK...l.
uint color_r = 0;
uint color_g = 0;
uint color_b = 0;
for (int p = -1; p < 2; p++)
{
for (int t = -1; t < 2; t++)
{
color_r += (0xFF & (colors[(rgb_height * (y_index + p)) + x_index + t] >> 16));
color_g += (0xFF & (colors[(rgb_height * (y_index + p)) + x_index + t] >> 8));
color_b += ((0xFF & colors[(rgb_height * (y_index + p)) + x_index + t] >> 0));
}
}
color_r = color_r / 9;
color_b = color_b / 9;
color_g = color_g / 9;
uint color = (color_r << 16) | (color_g << 8) | (color_b << 0);
//uint color = colors[(rgb_height * y_index) + x_index];
if (pcpoint.Id > 0)
{
pointCache[pcpoint.Id] = new Point
{
X = pcpoint.Position.x,
Y = pcpoint.Position.y,
Z = pcpoint.Position.z,
Id = pcpoint.Id,
Color = color
};
}
}
}
Debug.Log("POINT CLOUD SIZE: " + Frame.PointCloud.PointCount);
}
readyForNextFrame = false;
}
}
}
public async Task sendPoints()
{
try
{
using (var call = client.RecordPoints())
{
bool requestSent = false;
int startIndex = 0;
if (Frame.PointCloud.PointCount > 1000)
{
startIndex = Frame.PointCloud.PointCount - 1000;
}
for (int i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint pcpoint = Frame.PointCloud.GetPointAsStruct(i);
Vector3 screenCoordinate = cam.WorldToScreenPoint(pcpoint.Position);
if (Vector3.Dot(transform.forward, pcpoint - cam.transform.position) >= 0)
{
// checking if screenPoint is inside the screen area:
if (Rect.MinMaxRect(0, 0, Screen.width, Screen.height).Contains(screenCoordinate))
{
// screenPoint is a valid screen point
int x_index = (int)screenCoordinate.x * rgb_width / cam.pixelWidth;
int y_index = (int)screenCoordinate.y * rgb_height / cam.pixelHeight;
uint color = colors[(rgb_width * y_index) + x_index];
if (pcpoint.Id > 0)
{
requestSent = true;
await call.RequestStream.WriteAsync(new Point
{
X = pcpoint.Position.x,
Y = pcpoint.Position.y,
Z = pcpoint.Position.z,
Id = pcpoint.Id,
Color = color
});
}
}
}
}
if (requestSent)
{
await call.RequestStream.CompleteAsync();
Confirmation result = await call.ResponseAsync;
}
pointList.Clear();
isCallComplete = true;
}
}
catch (RpcException e)
{
Debug.Log("RPC failed");
throw;
}
}
//-----------------------------------------------------------------------
public void Update(PointCloudPoint pt)
{
pointCloud[pt.Id] = pt;
}
void Update()
{
if (!isInitialized)
{
return;
}
// check for errors
_QuitOnConnectionErrors();
// check for input (touch)
CheckForInputAction();
// estimate the tracking state
SessionStatus status = Session.Status;
if (status.IsError() || status.IsNotInitialized())
{
cameraTrackingState = TrackingState.Stopped;
return;
}
else if (status == SessionStatus.Tracking)
{
cameraTrackingState = TrackingState.Tracking;
}
else
{
cameraTrackingState = TrackingState.Paused;
}
// get frame timestamp and light intensity
lastFrameTimestamp = GetCurrentTimestamp();
if (Frame.LightEstimate.State == LightEstimateState.Valid)
{
// Normalize pixel intensity by middle gray in gamma space.
const float middleGray = 0.466f;
currentLightIntensity = Frame.LightEstimate.PixelIntensity / middleGray;
}
// get point cloud, if needed
MultiARInterop.MultiARData arData = arManager.GetARData();
if (arManager.usePointCloudData)
{
if (Frame.PointCloud.PointCount > 0 && Frame.PointCloud.IsUpdatedThisFrame)
{
// Copy the point cloud points
for (int i = 0; i < Frame.PointCloud.PointCount; i++)
{
PointCloudPoint point = Frame.PointCloud.GetPointAsStruct(i);
arData.pointCloudData[i] = new Vector3(point.Position.x, point.Position.y, point.Position.z);
}
arData.pointCloudLength = Frame.PointCloud.PointCount;
arData.pointCloudTimestamp = lastFrameTimestamp;
}
}
// // display the tracked planes if needed
// if(arManager.displayTrackedSurfaces && trackedPlanePrefab)
// {
// // get the new planes
// Frame.GetNewPlanes(ref newTrackedPlanes);
//
// // Iterate over planes found in this frame and instantiate corresponding GameObjects to visualize them.
// for (int i = 0; i < newTrackedPlanes.Count; i++)
// {
// // Instantiate a plane visualization prefab and set it to track the new plane.
// GameObject planeObject = Instantiate(trackedPlanePrefab, Vector3.zero, Quaternion.identity);
// planeObject.GetComponent<GoogleARCore.HelloAR.TrackedPlaneVisualizer>().SetTrackedPlane(newTrackedPlanes[i]);
//
// // Apply a random color and grid rotation.
// planeObject.GetComponent<Renderer>().material.SetColor("_GridColor", planeColors[Random.Range(0, planeColors.Length - 1)]);
// planeObject.GetComponent<Renderer>().material.SetFloat("_UvRotation", Random.Range(0.0f, 360.0f));
// }
// }
// get all tracked planes
Session.GetTrackables <DetectedPlane>(allTrackedPlanes, TrackableQueryFilter.All);
// create overlay surfaces as needed
if (arManager.useOverlaySurface != MultiARManager.SurfaceRenderEnum.None)
{
alSurfacesToDelete.Clear();
alSurfacesToDelete.AddRange(arData.dictOverlaySurfaces.Keys);
// estimate the material
Material surfaceMat = arManager.GetSurfaceMaterial();
int surfaceLayer = MultiARInterop.GetSurfaceLayer();
for (int i = 0; i < allTrackedPlanes.Count; i++)
{
string surfId = allTrackedPlanes[i].m_TrackableNativeHandle.ToString();
if (!arData.dictOverlaySurfaces.ContainsKey(surfId))
{
GameObject overlaySurfaceObj = new GameObject();
overlaySurfaceObj.name = "surface-" + surfId;
overlaySurfaceObj.layer = surfaceLayer;
overlaySurfaceObj.transform.SetParent(arData.surfaceRendererRoot ? arData.surfaceRendererRoot.transform : null);
// GameObject overlayCubeObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
// overlayCubeObj.name = "surface-cube-" + surfId;
// overlayCubeObj.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
// overlayCubeObj.transform.SetParent(overlaySurfaceObj.transform);
OverlaySurfaceUpdater overlaySurface = overlaySurfaceObj.AddComponent <OverlaySurfaceUpdater>();
overlaySurface.SetSurfaceMaterial(surfaceMat);
overlaySurface.SetSurfaceCollider(arManager.surfaceCollider, arManager.colliderMaterial);
arData.dictOverlaySurfaces.Add(surfId, overlaySurface);
}
// update the surface mesh
bool bValidSurface = UpdateOverlaySurface(arData.dictOverlaySurfaces[surfId], allTrackedPlanes[i]);
if (bValidSurface && alSurfacesToDelete.Contains(surfId))
{
alSurfacesToDelete.Remove(surfId);
}
}
// delete not tracked surfaces
foreach (string surfId in alSurfacesToDelete)
{
OverlaySurfaceUpdater overlaySurface = arData.dictOverlaySurfaces[surfId];
arData.dictOverlaySurfaces.Remove(surfId);
Destroy(overlaySurface.gameObject);
}
}
// check status of the anchors
List <string> alAnchorsToRemove = new List <string>();
foreach (string anchorId in arData.allAnchorsDict.Keys)
{
List <GameObject> anchoredObjs = arData.allAnchorsDict[anchorId];
foreach (GameObject anchoredObj in anchoredObjs)
{
Transform parentTrans = anchoredObj.transform.parent;
if (parentTrans == null)
{
if (!alAnchorsToRemove.Contains(anchorId))
{
alAnchorsToRemove.Add(anchorId);
}
anchoredObj.SetActive(false);
}
else
{
Anchor anchor = parentTrans.GetComponent <Anchor>();
if (anchor == null || anchor.TrackingState == TrackingState.Stopped)
{
if (!alAnchorsToRemove.Contains(anchorId))
{
alAnchorsToRemove.Add(anchorId);
}
anchoredObj.transform.parent = null;
anchoredObj.SetActive(false);
}
}
}
}
// remove the stopped anchors from our list
foreach (string anchorId in alAnchorsToRemove)
{
arData.allAnchorsDict.Remove(anchorId);
}
// clean up
alAnchorsToRemove.Clear();
// look for image anchors, if enabled
if (arData.imageAnchorsEnabled)
{
// Get updated augmented images for this frame.
Session.GetTrackables <AugmentedImage>(alTrackedAugmentedImages, TrackableQueryFilter.Updated);
foreach (var image in alTrackedAugmentedImages)
{
string sImageName = image.Name;
bool wasImageTracked = dictImageAnchors.ContainsKey(sImageName);
if (!wasImageTracked && image.TrackingState == TrackingState.Tracking)
{
// Create an anchor to ensure that ARCore keeps tracking this augmented image.
Anchor anchor = image.CreateAnchor(image.CenterPose);
anchor.gameObject.name = "ImageAnchor-" + sImageName;
DontDestroyOnLoad(anchor.gameObject);
alImageAnchorNames.Add(sImageName);
dictImageAnchors.Add(sImageName, anchor.gameObject);
}
else if (wasImageTracked && image.TrackingState == TrackingState.Stopped)
{
// remove the anchor
GameObject anchorObj = dictImageAnchors[sImageName];
alImageAnchorNames.Remove(sImageName);
dictImageAnchors.Remove(sImageName);
GameObject.Destroy(anchorObj);
}
}
}
}
public void Execute(int index)
{
int *data = (int *)(Input + Stride * index);
double x = data[0] * InputScaleX + InputOffsetX;
double y = data[1] * InputScaleY + InputOffsetY;
double z = data[2] * InputScaleZ + InputOffsetZ;
x = (x + OutputCenterX) * OutputScaleX;
y = (y + OutputCenterY) * OutputScaleY;
z = (z + OutputCenterZ) * OutputScaleZ;
var pt = Transform.MultiplyVector(new Vector3((float)x, (float)y, (float)z));
byte intensity;
{
ushort *iptr = (ushort *)(Input + Stride * index + 12);
if (LasRGB8BitWorkaround)
{
intensity = (byte)*iptr;
}
else
{
intensity = (byte)(*iptr >> 8);
}
}
uint color = (uint)(intensity << 24);
if (ColorInput == null)
{
color |= (uint)((intensity << 16) | (intensity << 8) | intensity);
}
else
{
ushort *rgb = (ushort *)(ColorInput + Stride * index);
if (LasRGB8BitWorkaround)
{
byte r = (byte)rgb[0];
byte g = (byte)rgb[1];
byte b = (byte)rgb[2];
color |= (uint)((b << 16) | (g << 8) | r);
}
else
{
byte r = (byte)(rgb[0] >> 8);
byte g = (byte)(rgb[1] >> 8);
byte b = (byte)(rgb[2] >> 8);
color |= (uint)((b << 16) | (g << 8) | r);
}
}
Output[index] = new PointCloudPoint()
{
Position = pt,
Color = color,
};
++Counts[ThreadIndex];
}
public void onDetectionAvailable(PointCloudPoint hit, DetectObjects.Detection d)
{
detectionCache[hit.Id] = new DetectionPoint
{
HitId = hit.Id,
HitOffset = 0,
ObjectClass = d.objectClass,
Confidence = d.confidence,
Pos = new Position {
X = hit.Position.x, Y = hit.Position.y, Z = hit.Position.z
},
Rot = new ProtoQuaternion {
X = 0, Y = 0, Z = 0, W = 0
},
Ex = new Extent {
X = 1, Y = 1
}
};
//Confirmation result = client.AddPointObject();
// PREVIOUS IMPLEMENTATION WITH Parameters (TrackableHit hit, Detection.Detection d)
// int hit_id = -1;
// float hit_offset = 0.5f;
// if (Frame.PointCloud.PointCount > 0)
// {
// for (var i = 0; i < Frame.PointCloud.PointCount; i++)
// {
// PointCloudPoint p = Frame.PointCloud.GetPointAsStruct(i);
// float dist = Vector3.Distance(hit.Pose.position, p.Position);
// if (dist < hit_offset)
// {
// hit_id = p.Id;
// hit_offset = dist;
// }
// }
// }
// if (hit.Trackable is DetectedPlane)
// {
// Pose p = ((DetectedPlane)(hit.Trackable)).CenterPose;
// Vector3 hp = hit.Pose.position;
// float exx = ((DetectedPlane)(hit.Trackable)).ExtentX;
// float exz = ((DetectedPlane)(hit.Trackable)).ExtentZ;
// Confirmation result = client.AddPlaneObject(new DetectionPlane
// {
// HitId = hit_id,
// HitOffset = hit_offset,
// ObjectClass = d.objectClass,
// Confidence = d.confidence,
// Pos = new Position { X = p.position.x, Y = p.position.y, Z = p.position.z },
// HitPos = new Position {X=hp.x,Y=hp.y,Z=hp.z},
// Rot = new ProtoQuaternion { X = p.rotation.x, Y = p.rotation.y, Z = p.rotation.z, W = p.rotation.w },
// Ex = new Extent { X = exx, Y = exz }
// });
// }
// else if (hit.Trackable is FeaturePoint)
// {
// FeaturePoint fp = (FeaturePoint)hit.Trackable;
// Pose p = fp.Pose;
// Vector3 lower_left = cam.ScreenToWorldPoint(new Vector3(d.boundingBox[1], d.boundingBox[0], hit.Distance));
// Vector3 upper_right = cam.ScreenToWorldPoint(new Vector3(d.boundingBox[3], d.boundingBox[2], hit.Distance));
// Vector3 upper_left = cam.ScreenToWorldPoint(new Vector3(d.boundingBox[1], d.boundingBox[2], hit.Distance));
// float exx = cam.pixelWidth * (upper_left - upper_right).magnitude;
// float exz = cam.pixelHeight * (upper_left - lower_left).magnitude;
// Confirmation result = client.AddPointObject(new DetectionPoint
// {
// HitId = hit_id,
// HitOffset = hit_offset,
// ObjectClass = d.objectClass,
// Confidence = d.confidence,
// Pos = new Position { X = p.position.x, Y = p.position.y, Z = p.position.z },
// Rot = new ProtoQuaternion { X = p.rotation.x, Y = p.rotation.y, Z = p.rotation.z, W = p.rotation.w },
// Ex = new Extent { X = exx, Y = exz }
// });
// }
}
/// <summary> /// Passes given point to one of the children. Child is chosen based on point's position. /// </summary> /// <param name="point">Point to pass.</param> protected abstract void PassPointToChild(PointCloudPoint point);
///<exclude/>
public bool Equals(PointCloudPoint other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return other._Point.Equals(_Point) && other._Colour.Equals(_Colour);
}
PointCloudData ImportLas(AssetImportContext context)
{
long size = new FileInfo(context.assetPath).Length;
using (var file = MemoryMappedFile.CreateFromFile(context.assetPath, FileMode.Open))
{
using (var view = file.CreateViewAccessor(0, size, MemoryMappedFileAccess.Read))
{
view.Read <LasHeader>(0, out var header);
if (Encoding.ASCII.GetString(header.Signature) != "LASF")
{
throw new Exception("Incorrect LAS file signature");
}
if (header.PointDataFormat > 10)
{
throw new Exception($"Unsupported LAS file format: {header.PointDataFormat}");
}
long offset = header.PointDataOffset;
int stride = header.PointDataSize;
long count = header.PointDataCount;
if (header.VersionMajor > 1 || header.VersionMajor == 1 && header.VersionMinor >= 4)
{
if (count == 0)
{
count = (long)header.PointDataCountLong;
}
}
if (count > MaxPointCount)
{
Debug.LogWarning($"Too many points ({count:n0}), truncating to {MaxPointCount:n0}");
count = MaxPointCount;
}
var bounds = new PointCloudBounds()
{
MinX = header.MinX,
MinY = header.MinY,
MinZ = header.MinZ,
MaxX = header.MaxX,
MaxY = header.MaxY,
MaxZ = header.MaxZ,
};
unsafe
{
byte *ptr = null;
view.SafeMemoryMappedViewHandle.AcquirePointer(ref ptr);
try
{
var points = new PointCloudPoint[(int)count];
fixed(PointCloudPoint *output = points)
{
bool hasColor = LasConvert(context, ptr + offset, stride, (int)count, ref header, bounds, output);
var transform = GetTransform();
var b = GetBounds(bounds);
b.center = transform.MultiplyPoint3x4(b.center);
b.extents = transform.MultiplyVector(b.extents);
return(PointCloudData.Create(points, GetBounds(bounds), hasColor, transform.MultiplyPoint3x4(bounds.Center), transform.MultiplyVector(bounds.Extents)));
}
}
finally
{
view.SafeMemoryMappedViewHandle.ReleasePointer();
}
}
}
}
}
PointCloudData ImportLaz(AssetImportContext context)
{
var name = Path.GetFileName(context.assetPath);
using (var laz = new Laszip(context.assetPath))
{
long count = laz.Count;
if (count > MaxPointCount)
{
Debug.LogWarning($"Too many points ({count:n0}), truncating to {MaxPointCount:n0}");
count = MaxPointCount;
}
var bounds = new PointCloudBounds()
{
MinX = laz.MinX,
MinY = laz.MinY,
MinZ = laz.MinZ,
MaxX = laz.MaxX,
MaxY = laz.MaxY,
MaxZ = laz.MaxZ,
};
double scaleX, scaleY, scaleZ;
double centerX, centerY, centerZ;
if (Normalize)
{
centerX = -0.5 * (bounds.MaxX + bounds.MinX);
centerY = -0.5 * (bounds.MaxY + bounds.MinY);
centerZ = -0.5 * (bounds.MaxZ + bounds.MinZ);
scaleX = 2.0 / (bounds.MaxX - bounds.MinX);
scaleY = 2.0 / (bounds.MaxY - bounds.MinY);
scaleZ = 2.0 / (bounds.MaxZ - bounds.MinZ);
}
else if (Center)
{
centerX = -0.5 * (bounds.MaxX + bounds.MinX);
centerY = -0.5 * (bounds.MaxY + bounds.MinY);
centerZ = -0.5 * (bounds.MaxZ + bounds.MinZ);
scaleX = scaleY = scaleZ = 1.0;
}
else
{
centerX = centerY = centerZ = 0.0;
scaleX = scaleY = scaleZ = 1.0;
}
bool hasColor = laz.HasColor;
var transform = GetTransform();
var points = new PointCloudPoint[(int)count];
try
{
for (long i = 0; i < count; i++)
{
if ((i % (128 * 1024)) == 0)
{
float progress = (float)((double)i / count);
EditorUtility.DisplayProgressBar($"Importing {name}", $"{i:N0} points", progress);
}
var p = laz.GetNextPoint();
double x = (p.X + centerX) * scaleX;
double y = (p.Y + centerY) * scaleY;
double z = (p.Z + centerZ) * scaleZ;
var pt = transform.MultiplyVector(new Vector3((float)x, (float)y, (float)z));
byte intensity;
if (LasRGB8BitWorkaround)
{
intensity = (byte)(p.Intensity >> 0);
}
else
{
intensity = (byte)(p.Intensity >> 8);
}
uint color = (uint)(intensity << 24);
if (hasColor)
{
if (LasRGB8BitWorkaround)
{
byte r = (byte)(p.Red >> 0);
byte g = (byte)(p.Green >> 0);
byte b = (byte)(p.Blue >> 0);
color |= (uint)((b << 16) | (g << 8) | r);
}
else
{
byte r = (byte)(p.Red >> 8);
byte g = (byte)(p.Green >> 8);
byte b = (byte)(p.Blue >> 8);
color |= (uint)((b << 16) | (g << 8) | r);
}
}
else
{
color |= (uint)((intensity << 16) | (intensity << 8) | intensity);
}
points[i] = new PointCloudPoint()
{
Position = pt,
Color = color,
};
}
}
finally
{
EditorUtility.ClearProgressBar();
}
var unityBounds = GetBounds(bounds);
unityBounds.center = transform.MultiplyPoint3x4(unityBounds.center);
unityBounds.extents = transform.MultiplyVector(unityBounds.extents);
return(PointCloudData.Create(points, unityBounds, hasColor, transform.MultiplyPoint3x4(bounds.Center), transform.MultiplyVector(bounds.Extents)));
}
}
