// Use this for initialization void Start() { PointCloudMetaData meta = new PointCloudMetaData(); meta.cloudName = ""; Node n = new Node("", meta, new BoundingBox(0, 0, 0, 10, 10, 10), null); Vector3[] vecs = new Vector3[12]; Color[] cols = new Color[12]; for (int i = 0; i < 12; i++) { vecs[i] = new Vector3(Mathf.Sin(i * Mathf.PI / 6), 0, Mathf.Cos(i * Mathf.PI / 6)); cols[i] = new Color(i / 12.0f, 1 - i / 12.0f, 1); } n.SetPoints(vecs, cols); n.CreateGameObjects(configuration); }
/* Loads the complete hierarchy of the given node. Creates all the children and their data. Points are not yet stored in there. * dataRPath is the path of the R-folder */ private static void LoadHierarchy(string dataRPath, PointCloudMetaData metaData, Node root) { byte[] data = FindAndLoadFile(dataRPath, metaData, root.Name, ".hrc"); int nodeByteSize = 5; int numNodes = data.Length / nodeByteSize; int offset = 0; Queue <Node> nextNodes = new Queue <Node>(); nextNodes.Enqueue(root); for (int i = 0; i < numNodes; i++) { Node n = nextNodes.Dequeue(); byte configuration = data[offset]; //uint pointcount = System.BitConverter.ToUInt32(data, offset + 1); //n.PointCount = pointcount; //TODO: Pointcount is wrong for (int j = 0; j < 8; j++) { //check bits if ((configuration & (1 << j)) != 0) { //This is done twice for some nodes Node child = new Node(n.Name + j, metaData, calculateBoundingBox(n.BoundingBox, j), n); n.SetChild(j, child); nextNodes.Enqueue(child); } } offset += 5; } HashSet <Node> parentsOfNextNodes = new HashSet <Node>(); while (nextNodes.Count != 0) { Node n = nextNodes.Dequeue().Parent; if (!parentsOfNextNodes.Contains(n)) { parentsOfNextNodes.Add(n); LoadHierarchy(dataRPath, metaData, n); } //Node n = nextNodes.Dequeue(); //LoadHierarchy(dataRPath, metaData, n); } }
private void LoadHierarchy() { try { if (!cloudPath.EndsWith("\\")) { cloudPath = cloudPath + "\\"; } PointCloudMetaData metaData = CloudLoader.LoadMetaData(cloudPath, false); setController.UpdateBoundingBox(this, metaData.boundingBox); Node rootNode = CloudLoader.LoadHierarchyOnly(metaData); setController.AddRootNode(rootNode); } catch (Exception ex) { Debug.LogError(ex); } }
private void LoadPoints(string dataRPath, PointCloudMetaData metaData, PotreeNode node) { byte[] data = FindAndLoadFile(dataRPath, metaData, node.Name, ".bin"); int pointByteSize = 16;//TODO: Is this always the case? int numPoints = data.Length / pointByteSize; int offset = 0; Vector3[] vertices = new Vector3[numPoints]; Color[] colors = new Color[numPoints]; //Read in data foreach (string pointAttribute in metaData.pointAttributes) { if (pointAttribute.Equals(PointAttributes.POSITION_CARTESIAN)) { for (int i = 0; i < numPoints; i++) { //Reduction to single precision! //Note: y and z are switched float x = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 0) * metaData.scale); float y = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 8) * metaData.scale); float z = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 4) * metaData.scale); //float x = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 0) * metaData.scale + node.PotreeBoundingBox.lx); // including bounding offset //float y = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 8) * metaData.scale + node.PotreeBoundingBox.lz); // including bounding offset //float z = (float)(System.BitConverter.ToUInt32(data, offset + i * pointByteSize + 4) * metaData.scale + node.PotreeBoundingBox.ly); // including bounding offset vertices[i] = new Vector3(x, y, z); } offset += 12; } else if (pointAttribute.Equals(PointAttributes.COLOR_PACKED)) { for (int i = 0; i < numPoints; i++) { byte r = data[offset + i * pointByteSize + 0]; byte g = data[offset + i * pointByteSize + 1]; byte b = data[offset + i * pointByteSize + 2]; colors[i] = new Color32(r, g, b, 255); } offset += 3; } } node.SetPoints(vertices, colors); }
private uint LoadAllPoints(string dataRPath, PointCloudMetaData metaData, PotreeNode node) { LoadPoints(dataRPath, metaData, node); uint numpoints = (uint)node.PointCount; // load until maxLevel if (node.GetLevel() >= meshConfiguration.maxLevel) { return(numpoints); } for (int i = 0; i < 8; i++) { if (node.HasChild(i)) { numpoints += LoadAllPoints(dataRPath, metaData, node.GetChild(i)); } } return(numpoints); }
/* Loads the metadata from the json-file in the given cloudpath */ /// <summary> /// Loads the meta data from the json-file in the given cloudpath. Attributes "cloudPath", and "cloudName" are set as well. /// </summary> /// <param name="cloudPath">Folderpath of the cloud</param> /// <param name="moveToOrigin">True, if the center of the cloud should be moved to the origin</param> public static PointCloudMetaData LoadMetaData(string cloudPath, bool moveToOrigin = false) { string jsonfile; Debug.Log(cloudPath); bool isCloudOnline = Uri.IsWellFormedUriString(cloudPath, UriKind.Absolute); if (isCloudOnline) { WebClient client = new WebClient(); Stream stream = client.OpenRead(cloudPath + "cloud.js"); StreamReader reader = new StreamReader(stream); jsonfile = reader.ReadToEnd(); } else { using (StreamReader reader = new StreamReader(cloudPath + "cloud.js", Encoding.Default)) { jsonfile = reader.ReadToEnd(); reader.Close(); } } PointCloudMetaData metaData = PointCloudMetaDataReader.ReadFromJson(jsonfile, moveToOrigin); metaData.cloudName = cloudPath.Substring(0, cloudPath.Length - 1).Substring(cloudPath.Substring(0, cloudPath.Length - 1).LastIndexOf("/") + 1); Debug.Log(metaData.cloudName); if (isCloudOnline) { metaData.cloudUrl = cloudPath; metaData.cloudPath = "temp/" + metaData.cloudName + "/"; } else { metaData.cloudPath = cloudPath; metaData.cloudUrl = null; } return(metaData); }
//Loads the complete point cloud private void LoadFile() { try { Debug.Log("Loading file"); fileLoading = true; if (!cloudPath.EndsWith("\\")) { cloudPath = cloudPath + "\\"; } metaData = CloudLoader.LoadMetaData(cloudPath, moveToOrigin); rootNode = CloudLoader.LoadPointCloud(metaData); Debug.Log("Finished Loading"); fileLoading = false; } catch (Exception ex) { Debug.LogError(ex); } }
/* Finds a file for a node in the hierarchy. * Assuming hierarchyStepSize is 3 and we are looking for the file 0123456765.bin, it is in: * 012/012345/012345676/r0123456765.bin * 012/345/676/r012345676.bin */ private static byte[] FindAndLoadFile(string dataRPath, PointCloudMetaData metaData, string id, string fileending) { int levels = metaData.hierarchyStepSize == 0 ? 0 : id.Length / metaData.hierarchyStepSize; string path = ""; for (int i = 0; i < levels; i++) { path += id.Substring(i * metaData.hierarchyStepSize, metaData.hierarchyStepSize) + "/"; } path += "r" + id + fileending; if (dataRPath.Contains("http")) { WebClient client = new WebClient(); return(client.DownloadData(dataRPath + path)); } else { return(File.ReadAllBytes(dataRPath + path)); } }
/* Finds a file for a node in the hierarchy. * Assuming hierarchyStepSize is 3 and we are looking for the file 0123456765.bin, it is in: * 012/012345/012345676/r0123456765.bin * 012/345/676/r012345676.bin */ private static byte[] FindAndLoadFile(string dataRPath, PointCloudMetaData metaData, string id, string fileending) { int levels = id.Length / metaData.hierarchyStepSize; string path = ""; for (int i = 0; i < levels; i++) { path += id.Substring(i * metaData.hierarchyStepSize, metaData.hierarchyStepSize) + "/"; } path += "r" + id + fileending; if (File.Exists(metaData.cloudPath + dataRPath + path)) { return(File.ReadAllBytes(metaData.cloudPath + dataRPath + path)); } else if (metaData.cloudUrl != null) { Directory.CreateDirectory(Path.GetDirectoryName(metaData.cloudPath + dataRPath + path)); WebClient webClient = new WebClient(); webClient.DownloadFile(metaData.cloudUrl + dataRPath + path, metaData.cloudPath + dataRPath + path); return(File.ReadAllBytes(metaData.cloudPath + dataRPath + path)); } return(null); }
public void SetPointCloudDataFromInteropData(PointCloudData <PointXYZ> pcd, PointCloudMetaData metadata) { throw new NotImplementedException(); }
void Update() { if (SaveEnabled) { saved = true; Vector3 newposition = gameObject.transform.position; GameObject directory = GameObject.Find("Directories"); GameObject dirctoryobject = directory.transform.Find(gameObject.name).gameObject; string cloud = dirctoryobject.GetComponent <DynamicLoaderController>().cloudPath; string jsonfile; using (StreamReader reader = new StreamReader(cloud + "cloud.js", Encoding.Default)) { jsonfile = reader.ReadToEnd(); reader.Close(); } PointCloudMetaData data = JsonUtility.FromJson <PointCloudMetaData>(jsonfile); if (Overwrite) { data.RotateX = 0f; data.RotateY = 0f; data.RotateZ = 0f; } else { data.RotateX += gameObject.transform.eulerAngles.x; data.RotateY += gameObject.transform.eulerAngles.y; data.RotateZ += gameObject.transform.eulerAngles.z; } if (data.ScaleX == 0) { data.ScaleX = 1; data.ScaleY = 1; data.ScaleZ = 1; } else { data.ScaleX *= gameObject.transform.localScale.x; data.ScaleY *= gameObject.transform.localScale.y; data.ScaleZ *= gameObject.transform.localScale.z; } Vector3 scale = CloudsFromDirectoryLoader.boxscale[gameObject.name]; data.boundingBox.lx *= data.ScaleX / scale.x; data.boundingBox.ly *= data.ScaleY / scale.y; data.boundingBox.lz *= data.ScaleZ / scale.z; data.boundingBox.ux *= data.ScaleX / scale.x; data.boundingBox.uy *= data.ScaleY / scale.y; data.boundingBox.uz *= data.ScaleZ / scale.z; data.tightBoundingBox.lx *= data.ScaleX / scale.x; data.tightBoundingBox.ly *= data.ScaleY / scale.y; data.tightBoundingBox.lz *= data.ScaleZ / scale.z; data.tightBoundingBox.ux *= data.ScaleX / scale.x; data.tightBoundingBox.uy *= data.ScaleY / scale.y; data.tightBoundingBox.uz *= data.ScaleZ / scale.z; data.boundingBox.olx *= data.ScaleX / scale.x; data.boundingBox.oly *= data.ScaleY / scale.y; data.boundingBox.olz *= data.ScaleZ / scale.z; data.boundingBox.oux *= data.ScaleX / scale.x; data.boundingBox.ouy *= data.ScaleY / scale.y; data.boundingBox.ouz *= data.ScaleZ / scale.z; Vector3d vector = ToVector3d(gameObject.transform.position); data.boundingBox.lx += vector.x - CloudsFromDirectoryLoader.boxoffset[gameObject.name].x; data.boundingBox.ly += vector.z - CloudsFromDirectoryLoader.boxoffset[gameObject.name].y; data.boundingBox.lz += vector.y - CloudsFromDirectoryLoader.boxoffset[gameObject.name].z; data.boundingBox.ux += vector.x - CloudsFromDirectoryLoader.boxoffset[gameObject.name].x; data.boundingBox.uy += vector.z - CloudsFromDirectoryLoader.boxoffset[gameObject.name].y; data.boundingBox.uz += vector.y - CloudsFromDirectoryLoader.boxoffset[gameObject.name].z; data.tightBoundingBox.lx += vector.x - CloudsFromDirectoryLoader.boxoffset[gameObject.name].x; data.tightBoundingBox.ly += vector.z - CloudsFromDirectoryLoader.boxoffset[gameObject.name].y; data.tightBoundingBox.lz += vector.y - CloudsFromDirectoryLoader.boxoffset[gameObject.name].z; data.tightBoundingBox.ux += vector.x - CloudsFromDirectoryLoader.boxoffset[gameObject.name].x; data.tightBoundingBox.uy += vector.z - CloudsFromDirectoryLoader.boxoffset[gameObject.name].y; data.tightBoundingBox.uz += vector.y - CloudsFromDirectoryLoader.boxoffset[gameObject.name].z; if (SetPivot) { data.boundingBox.olx = data.boundingBox.lx; data.boundingBox.oly = data.boundingBox.ly; data.boundingBox.olz = data.boundingBox.lz; data.boundingBox.oux = data.boundingBox.ux; data.boundingBox.ouy = data.boundingBox.uy; data.boundingBox.ouz = data.boundingBox.uz; } string output = JsonUtility.ToJson(data); File.WriteAllText(cloud + "cloud.js", output); Debug.Log("updated!"); SaveEnabled = false; } if (restart) { GeoQuadMeshConfiguration.rotatelist.Clear(); var script = GameObject.Find("PointSetController").GetComponent <PointCloudSetRealTimeController>(); script.PointRenderer.ShutDown(); SceneManager.LoadScene(SceneManager.GetActiveScene().buildIndex); } }
protected override IEnumerator ActivateCoroutine() { //Change the max tilt angle of the camera GameObject playerObj = GameObject.FindGameObjectWithTag("Player"); Player player = playerObj.GetComponent <Player>(); player.ChangeTiltAngle(85.0f); Debug.Log("Activating points"); //create/activate pointcloudsetrealtimecontroller SerializableModel pointsData = siteData as SerializableModel; Debug.Log("Loading points " + pointsData.name); Debug.Log("Initializing mesh"); mesh = this.gameObject.AddComponent <GeoTriMeshConfiguration>(); mesh.pointRadius = 5; mesh.renderCircles = true; mesh.screenSize = true; mesh.interpolation = InterpolationMode.OFF; mesh.reloadingPossible = true; Debug.Log("Initializing set"); set = this.gameObject.AddComponent <PointCloudSetRealTimeController>(); set.moveCenterToTransformPosition = true; set.pointBudget = 4294967295; set.minNodeSize = 10; set.nodesLoadedPerFrame = 15; set.nodesGOsPerFrame = 30; set.meshConfiguration = mesh; set.cacheSizeInPoints = 3000000; set.userCamera = CAVECameraRig.frustumCamera; Debug.Log("Initializing controller"); //create dynamicloadercontroller controller = this.gameObject.AddComponent <DynamicLoaderController>(); controller.cloudPath = GetCacheDirectory(pointsData.file); controller.setController = set; set.userCamera = CAVECameraRig.frustumCamera; yield return(null); if (this.spoi != null) { try { string cloudPath = GetCacheDirectory(pointsData.file); if (!cloudPath.EndsWith("\\")) { cloudPath = cloudPath + "\\"; } PointCloudMetaData md = CloudLoader.LoadMetaData(cloudPath, false); this.spoi.Delete(); this.spoi = new ShapefilePOI(this.siteData.custom.shapefilePath, -md.boundingBox.Center().ToFloatVector()); } catch (Exception ex) { Debug.LogError(ex); } } }
private PointCloudData <T> ParseLines(FileInfo f, int id) { _metadata = new PointCloudMetaData(); bool isInitialized = false; int index = 0; _startData = false; T[] points = new T[0]; string[] lines = new string[0]; try { lines = File.ReadAllLines(f.FullName); } catch (Exception e) { UnityEngine.Debug.Log(e.Message); } foreach (string line in lines) { // Parse metadata if (!_startData) { ParseMetadata(line, ref _metadata); } else { if (!isInitialized) { points = new T[_metadata.pointSize]; isInitialized = true; } // Parse vertices string[] values = line.Split(' '); if (values.Length != _metadata.numFields) { throw new System.Exception("Vertices was not same as fields"); } float x = 0, y = 0, z = 0; bool success = float.TryParse(values[0], out x); success = success && float.TryParse(values[1], out y); success = success && float.TryParse(values[2], out z); if (!success) { throw new System.Exception("Could not parse a vertex"); } if (_metadata.field == PointCloudDataType.XYZ) { Vector3 pos = new Vector3(x, y, z); PointXYZ point = new PointXYZ(pos); points[index++] = (T)point; } /* * else if (_metadata.field == PointCloudDataType.XYZRGB) * { * float rgba = float.Parse(values[3]); * byte r, g, b; * * // Unpack packed float - 3 channel * byte[] colorBytes = BitConverter.GetBytes(rgba); * r = colorBytes[0]; * g = colorBytes[1]; * b = colorBytes[2]; * * Vector3 point = new Vector3(x, y, z); * Color col = new Color(r, g, b); * points[index++] = new PointXYZRGBA(point, col); * } * else if (_metadata.field == PointCloudDataType.XYZRGBA) * { * int rgba = int.Parse(values[3]); * byte r, g, b, a; * * // Unpack float - 4 channel * byte[] colorBytes = BitConverter.GetBytes(rgba); * r = colorBytes[0]; * g = colorBytes[1]; * b = colorBytes[2]; * a = colorBytes[3]; * * Vector3 point = new Vector3(x, y, z); * Color32 col = new Color32(r, g, b, a); * points[index++] = new PointXYZRGBA(point, col); * } */ } } if (points.Length == 0 || points.Length != _metadata.pointSize) { throw new System.Exception("Vertices were empty or did not match declared points. Was:" + points.Length + ", but expected: " + _metadata.pointSize); } PointCloudData <T> pcd = new PointCloudData <T>(points, points.Length, _metadata, id); return(pcd); }
protected void ParseMetadata(string line, ref PointCloudMetaData metadata) { string[] values = line.Split(' '); switch (values[0]) { case "FIELDS": string field = line.Substring(7); if (field.Equals("x y z rgb")) { metadata.field = PointCloudDataType.XYZRGB; metadata.numFields = 4; } else if (field.Equals("x y z rgba")) { metadata.field = PointCloudDataType.XYZRGBA; metadata.numFields = 4; } else if (field.Equals("x y z normal_x normal_y normal_z")) { metadata.field = PointCloudDataType.XYZNORMALS; metadata.numFields = 6; } else if (field.Equals("x y z")) { metadata.field = PointCloudDataType.XYZ; metadata.numFields = 3; } else { throw new System.Exception("Fields is not supported or was not parsed correctly."); } metadata.InitializeDataFields(); break; case "SIZE": if (values.Length != metadata.numFields + 1) { throw new System.Exception("Count could not be parsed correctly."); } for (int i = 0; i < metadata.numFields; i++) { if (!int.TryParse(values[i + 1], out metadata.fieldSize[i])) { throw new System.Exception("Size could not be parsed correctly."); } } break; case "TYPE": if (values.Length != metadata.numFields + 1) { throw new System.Exception("Count could not be parsed correctly."); } for (int i = 0; i < metadata.numFields; i++) { if (!char.TryParse(values[i + 1], out metadata.fieldType[i])) { throw new System.Exception("Type could not be parsed correctly."); } } break; case "COUNT": if (values.Length != metadata.numFields + 1) { throw new System.Exception("Count could not be parsed correctly."); } for (int i = 0; i < metadata.numFields; i++) { if (!int.TryParse(values[i + 1], out metadata.fieldCount[i])) { throw new System.Exception("Count could not be parsed correctly."); } } break; case "WIDTH": if (!int.TryParse(values[1], out metadata.width)) { throw new System.Exception("Count could not be parsed correctly."); } break; case "HEIGHT": if (!int.TryParse(values[1], out metadata.height)) { throw new System.Exception("Count could not be parsed correctly."); } break; case "VIEWPOINT": float[] viewpoints = new float[7]; for (int i = 1; i < values.Length; i++) { if (!float.TryParse(values[i], out viewpoints[i - 1])) { throw new System.Exception("Viewpoint could not be parsed correctly."); } } metadata.SetViewpointPosition(viewpoints[0], viewpoints[1], viewpoints[2]); metadata.SetViewpointRotation(viewpoints[3], viewpoints[4], viewpoints[5], viewpoints[6]); break; case "POINTS": if (!int.TryParse(values[1], out metadata.pointSize)) { throw new System.Exception("Points could not be parsed correctly."); } break; case "DATA": _startData = true; break; } }
void Update() { GameObject myObject = GameObject.Find("object"); GameObject myCam = GameObject.Find("myCamera"); GameObject my_real_object = GameObject.Find("realObject"); GameObject Line = GameObject.Find("Line"); GameObject arrow = GameObject.Find("TransArrow"); GameObject rotLine1 = GameObject.Find("rotLine1"); GameObject rotLine2 = GameObject.Find("rotLine2"); GameObject text = GameObject.Find("text"); GameObject success = GameObject.Find("success"); //var line = my_real_object.AddComponent<LineRenderer>(); //Transform my_real_object; if (pause == false) { //Debug.Log("start the app"); Time.timeScale = 1; //Debug.Log("start the main function"); //Time.timeScale = 1; cnt = cnt + 1; if (cnt % updateRate == 0) { //Debug.Log("enter the pointclouddata"); PointCloudMetaData metadata = new PointCloudMetaData(); metaContext.Get <InteractionEngine>().GetCloudMetaData(ref metadata); PointCloudData <PointXYZConfidence> pointCloudData = new PointCloudData <PointXYZConfidence>(metadata.maxSize); metaContext.Get <InteractionEngine>().GetCloudData(ref pointCloudData); //Debug.Log(pointCloudData.size); //Use point cloud data here /* * for (int i = 0; i < pointCloudData.size; i++) * { * string pointinfo = pointCloudData.points[i].ToString(); * Debug.Log(i); * System.IO.File.WriteAllText(path: @"D:\test1.txt", contents: pointinfo); * }*/ /* * var type = pointCloudData.points[0].vertex.GetType(); * Debug.Log(type); * var sizetype = pointCloudData.points[0].vertex[0].GetType(); * Debug.Log("xxxxxxx"+sizetype); */ int length = pointCloudData.points.Length; int size = pointCloudData.size; //Debug.Log("point_size=" + size); //Debug.Log("point_length=" + length); float[] psource = new float[size * 3]; float[] transInfo = new float[7]; unsafe { fixed(float *temp_p = psource) { fixed(float *transinfo = transInfo) { float *pt = temp_p; int num = 0; foreach (PointXYZConfidence point in pointCloudData.points) { for (int i = 0; i < 3; i++) { if (i == 0) { *pt = point.vertex[i]; } else if (i == 1) { *pt = -point.vertex[i]; } else { *pt = point.vertex[i]; } pt++; num++; } if (num >= size * 3) { break; } } //Debug.Log("num:" + num); pt = null; if (pause2 == false) { // get cuurent pose of real object float[] temp_real = new float[8]; fixed(float *initial_guess_ = real_obj) { temp_real = dataConverter(temp_p, num, initial_guess_, isFirst); } if (temp_real[0] == -1) { return; } // store the result in C# format Quaternion rot_real = new Quaternion(temp_real[1], temp_real[2], temp_real[3], temp_real[0]); Vector3 trans_real = new Vector3(temp_real[4], temp_real[5], temp_real[6]); float score = temp_real[7]; // get last valid pose Vector3 real_obj_trans = new Vector3(real_obj[4], real_obj[5], real_obj[6]); Quaternion real_obj_rot = new Quaternion(real_obj[1], real_obj[2], real_obj[3], real_obj[0]); // translational distance between current and last pose estimation float frame_dist = (prev_frame_trans - trans_real).magnitude; bool inFOV = trans_real.z > 0.12 && trans_real.x <trans_real.z && trans_real.x> -trans_real.z && trans_real.y <0.6 * trans_real.z && trans_real.y> -0.6 * trans_real.z; // continue if the icp position is at good location if (score < 0.001 && frame_dist < 0.1 && inFOV) // { if (isFirst) { for (int i = 0; i < 8; i++) { real_obj[i] = temp_real[i]; } } updateRate = 10; // whether curent pose estimation is close to last valid pose bool close_to_last = (trans_real - real_obj_trans).magnitude < 0.1 && (Quaternion.Dot(real_obj_rot, rot_real) > 0.65);//==================== if (close_to_last || isFirst) { for (int i = 0; i < 8; i++) { real_obj[i] = temp_real[i]; } // ================== update real object location and draw lines ===================== my_real_object.transform.rotation = myCam.transform.rotation; my_real_object.transform.position = myCam.transform.position; my_real_object.transform.Translate(trans_real, Space.Self); my_real_object.transform.Rotate(rot_real.eulerAngles, Space.Self); var line = Line.GetComponent <LineRenderer>(); line.sortingOrder = 1; var rotline1 = rotLine1.GetComponent <LineRenderer>(); rotline1.sortingOrder = 1; var rotline2 = rotLine2.GetComponent <LineRenderer>(); rotline2.sortingOrder = 1; var myArrow = arrow.GetComponent <LineRenderer>(); myArrow.sortingOrder = 1; Quaternion offset1 = new Quaternion(0, 0, 1, 0); Quaternion offset2 = new Quaternion(0, 1, 0, 0); Quaternion rot_diff = Quaternion.Inverse(my_real_object.transform.rotation * offset1) * myObject.transform.rotation * offset2; Vector3 rot_axis = new Vector3(0, 0, 0); float rot_angle = 0; rot_diff.ToAngleAxis(out rot_angle, out rot_axis); // -------------draw translational distance------------------- var point1 = my_real_object.transform.position; var point2 = myObject.transform.position; point1.y += 0.05f; point2.y += 0.05f; Vector3 point3 = Vector3.Lerp(point1, point2, 0.7f); Vector3 point4 = Vector3.Lerp(point1, point2, 0.9f); line.positionCount = 2; myArrow.positionCount = 2; // set color accroding to distance var distance_real_to_model = (point1 - point2).magnitude; if (distance_real_to_model > 0.15) { line.material.color = Color.red; myArrow.material.color = Color.red; } else if (distance_real_to_model > 0.1) { line.material.color = Color.yellow; myArrow.material.color = Color.yellow; } else { line.material.color = Color.green; myArrow.material.color = Color.green; } line.SetPosition(0, point1); line.SetPosition(1, point3); line.SetWidth(0.0125f, 0.0125f); line.useWorldSpace = true; myArrow.SetPosition(0, point3); myArrow.SetPosition(1, point4); myArrow.SetWidth(0.025f, 0.0f); line.useWorldSpace = true; // -------------------draw rotational distance ----------------------- var point1r = point1; point1r = point1r - 0.2f * my_real_object.transform.right; var point2r = point1; point2r = point2r - 0.2f * myObject.transform.right; var visualCo = 2.0f; point2r.x += (point2.x - point1.x); // / (point2 - point1).magnitude; point2r.y += (point2.y - point1.y); // / (point2 - point1).magnitude; point2r.z += (point2.z - point1.z); // / (point2 - point1).magnitude; rotline1.positionCount = 2; rotline1.SetPosition(0, point1); rotline1.SetPosition(1, point1r); rotline1.SetWidth(0.005f, 0.005f); rotline1.useWorldSpace = true; rotline1.material.color = Color.red; rotline2.positionCount = 2; rotline2.SetPosition(0, point1); rotline2.SetPosition(1, point2r); rotline2.SetWidth(0.005f, 0.005f); rotline2.useWorldSpace = true; rotline2.material.color = Color.yellow; // ----------------visualize the distance error--------------------------- //var distance_display = (point1 - point2).magnitude; countDist.text = "Matching Complete"; //TextMesh t = text.AddComponent<TextMesh>(); // //t.text = //t.fontSize = 30; //t.transform.localEulerAngles += new Vector3(90, 0, 0); //var point2text = point2; //point2text.y += 0.1f; countDist.transform.position = myObject.transform.position; //==============================visualization end ======================================= if (distance_real_to_model < 0.03 && (Quaternion.Dot(real_obj_rot, rot_real) > 0.85)) { success.transform.position = myObject.transform.position - myObject.transform.right * 0.05f + myObject.transform.up * 0.15f; } } isFirst = false; } prev_frame_trans = trans_real; pre_frame_rot = rot_real; pre_score = score; } else { detectplane(temp_p, num, transinfo); Vector3 trans_cam2obj = new Vector3(transinfo[0], transinfo[1], transinfo[2]); Quaternion rot_cam2obj = new Quaternion(transinfo[3], transinfo[4], transinfo[5], transinfo[6]); //Vector3 trans_world2cam = myCam.transform.position; //Quaternion rot_world2cam = myCam.transform.rotation;// * tempQ; myObject.transform.rotation = myCam.transform.rotation; myObject.transform.position = myCam.transform.position; myObject.transform.Translate(trans_cam2obj, Space.Self); myObject.transform.Rotate(rot_cam2obj.eulerAngles, Space.Self); //------------------------------------------- //myObject.transform.rotation = rot_world2cam; //myObject.transform.position = trans_world2cam; //Quaternion new_rot_cam2obj = Quaternion.Inverse(rot_cam2obj); //myObject.transform.localPosition = trans_cam2obj; //myObject.transform.localRotation = rot_cam2obj; //myObject.transform.rotation = rot_world2cam*rot_cam2obj; //myObject.transform.position = myCam.transform.TransformPoint(trans_cam2obj); //Debug.Log("xyz"+myObject.transform.position.x + myObject.transform.position.y + myObject.transform.position.z); } } } } } } else { Time.timeScale = 1; } if (Input.GetKeyDown(KeyCode.P)) { if (pause == true) { pause = false; } else { pause = false; } } if (Input.GetKeyDown(KeyCode.U)) { if (pause == true) { pause = true; } else { pause = true; } } if (Input.GetKeyDown(KeyCode.O)) { if (pause2 == true) { pause2 = false; } else { pause2 = false; } } if (Input.GetKeyDown(KeyCode.I)) { if (pause2 == true) { pause2 = true; } else { pause2 = true; } } if (Input.GetKeyDown(KeyCode.Space)) { isFirst = true; } }
/// <summary> /// Reads the specified file and returns PointCloudData, assigning the given frame id. /// </summary> /// <param name="f"></param> public virtual PointCloudData <PointXYZConfidence> ParseFile(FileInfo f, int id) { _metadata = new PointCloudMetaData(); bool isInitialized = false; int index = 0; _startData = false; PointXYZConfidence[] points = new PointXYZConfidence[0]; string[] lines = new string[0]; int numPoints = 0; try { lines = File.ReadAllLines(f.FullName); } catch (Exception e) { UnityEngine.Debug.Log(e.Message); } foreach (string line in lines) { // Parse metadata if (!_startData) { ParseMetadata(line, ref _metadata); } else { if (!isInitialized) { numPoints = _metadata.pointSize; if (_metadata.pointSize != lines.Length - 11) { numPoints = lines.Length - 11; UnityEngine.Debug.Log("Metadata point count didn't match. Overriding point count."); } points = new PointXYZConfidence[numPoints]; isInitialized = true; } // Parse vertices string[] values = line.Split(' '); if (values.Length != _metadata.numFields) { throw new System.Exception("Vertices was not same as fields"); } float x = 0, y = 0, z = 0, conf = 0; bool success = float.TryParse(values[0], out x); success = success && float.TryParse(values[1], out y); success = success && float.TryParse(values[2], out z); success = success && float.TryParse(values[3], out conf); //-3.402823E+38 -3.402823E+38 -3.402823E+38 0 // Watch for these values - causes Unity AABB error. Skip for now. To investigate. if (conf == 0) { points[index++] = new PointXYZConfidence(Vector3.zero, 0); continue; } if (!success) { throw new System.Exception("Could not parse a vertex"); } Vector3 pos = new Vector3(x, y, z); PointXYZConfidence point = new PointXYZConfidence(pos, conf); points[index++] = point; } } // Fixing metadata _metadata.maxSize = MaxPoints; if (points.Length == 0) { throw new System.Exception("Vertices were empty"); } PointCloudData <PointXYZConfidence> pcd = new PointCloudData <PointXYZConfidence>(points, numPoints, _metadata, id); return(pcd); }