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