/// <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))); } }