private void AnnotateButtonClick()
{
// TODO: check if tracking
byte[] png;
if (DebugImage != null)
{
png = DebugImage.EncodeToPNG();
}
else
{
png = ArUtils.GetCameraImage(ARInterface.GetInterface());
if (png == null)
{
return;
}
}
var imageModel = new ImageModel(png);
imageModel.Save();
var annotationModel =
new AnnotationModel(
new Pose(Camera.main.transform.position, Camera.main.transform.rotation),
Camera.main.aspect,
imageModel);
annotationModel.Save();
AddAnnotation(annotationModel);
}
// Update is called once per frame
void Update()
{
// Send pose update
_deltaTimePose += Time.unscaledDeltaTime;
if (_deltaTimePose >= 1.0 / 5)
{
if (ARInterface.GetInterface().TryGetPose(ref _pose))
{
SaveData("pose", _pose);
_deltaTimePose = 0;
}
}
// Send point cloud update
_deltaTimePointCloud += Time.unscaledDeltaTime;
if (_deltaTimePointCloud >= 1.0 / 5)
{
if (ARInterface.GetInterface().TryGetPointCloud(ref _pointCloud))
{
var data = _pointCloud.points.ToArray();
SaveData("pointcloud", data);
_deltaTimePointCloud = 0;
}
}
}
// Update is called once per frame
void Update()
{
if (ARInterface.GetInterface().TryGetPointCloud(ref m_PointCloud))
{
var scale = GetScale();
var numParticles = Mathf.Min(m_PointCloud.points.Count, m_MaxPointsToShow);
if (m_Particles == null || m_Particles.Length != numParticles)
{
m_Particles = new ParticleSystem.Particle[numParticles];
}
for (int i = 0; i < numParticles; ++i)
{
m_Particles[i].position = m_PointCloud.points[i] * scale;
m_Particles[i].startColor = new Color(1.0f, 1.0f, 1.0f);
m_Particles[i].startSize = m_ParticleSize * scale;
}
m_ParticleSystem.SetParticles(m_Particles, numParticles);
}
else
{
m_ParticleSystem.SetParticles(m_NoParticles, 1);
}
}
private void Start()
{
_arInterface = ARInterface.GetInterface();
m_ParticleSystem = Instantiate(m_PointCloudParticlePrefab, Camera.main.transform.parent);
_surface = GameObject.CreatePrimitive(PrimitiveType.Quad);
var mesh = _surface.GetComponent <MeshFilter>().mesh;
MeshHelper.Subdivide(mesh, MeshSize - 1);
_surface.GetComponent <MeshFilter>().mesh = mesh;
// _surface.transform.parent = Camera.main.transform;
// _surface.transform.localScale = new Vector3(Camera.main.aspect, 1, 1);
// _surface.transform.position = new Vector3(0, 0, 1);
_vertices = mesh.vertices;
_verticesIdx = new int[MeshSize, MeshSize];
_verticesZ = new double[MeshSize, MeshSize];
_verticesWeight = new double[MeshSize, MeshSize];
for (int i = 0; i < mesh.vertices.Length; i++)
{
var u = (int)Math.Round((mesh.vertices[i].x + 0.5) * (MeshSize - 1));
var v = (int)Math.Round((mesh.vertices[i].y + 0.5) * (MeshSize - 1));
_verticesIdx[u, v] = i;
}
}
// Update is called once per frame
void Update()
{
if (particlesOn && ARInterface.GetInterface().TryGetPointCloud(ref m_PointCloud))
{
var scale = pointsRoot.transform.localScale.x;
//CommandKeeper.WriteLineDebug("scale = " + scale);
var numParticles = Mathf.Min(m_PointCloud.points.Count, m_MaxPointsToShow);
if (m_Particles == null || m_Particles.Length != numParticles)
{
m_Particles = new ParticleSystem.Particle[numParticles];
}
for (int i = 0; i < numParticles; ++i)
{
m_Particles[i].position = m_PointCloud.points[i] * scale;
m_Particles[i].startColor = new Color(1.0f, 1.0f, 1.0f);
m_Particles[i].startSize = m_ParticleSize * scale;
}
m_ParticleSystem.SetParticles(m_Particles, numParticles);
}
else
{
m_ParticleSystem.SetParticles(m_NoParticles, 1);
}
}
// Update is called once per frame
void Update()
{
// Send pose update
_deltaTimePose += Time.unscaledDeltaTime;
if (_deltaTimePose >= 1.0 / 30)
{
if (ARInterface.GetInterface().TryGetPose(ref _pose))
{
var data = JsonUtility.ToJson(_pose);
SendData("pose", data);
_deltaTimePose = 0;
}
}
// Send point cloud update
_deltaTimePointCloud += Time.unscaledDeltaTime;
if (_deltaTimePointCloud >= 1.0 / 10)
{
if (ARInterface.GetInterface().TryGetPointCloud(ref _pointCloud))
{
var data = string.Join(";", _pointCloud.points
.Select(x => new SerializableVector3(x.x, x.y, x.z))
.Select(JsonUtility.ToJson)
.ToArray());
SendData("pointcloud", data);
_deltaTimePointCloud = 0;
}
}
}
// Update is called once per frame
void Update()
{
// Send pose update
_deltaTimePose += Time.unscaledDeltaTime;
if (_deltaTimePose >= 1.0 / 5)
{
if (ARInterface.GetInterface().TryGetPose(ref _pose))
{
SendData("pose", _pose);
_deltaTimePose = 0;
}
}
// Send point cloud update
_deltaTimePointCloud += Time.unscaledDeltaTime;
if (_deltaTimePointCloud >= 1.0 / 5)
{
if (ARInterface.GetInterface().TryGetPointCloud(ref _pointCloud))
{
var data = _pointCloud.points.ToArray();
SendData("pointcloud", data);
_deltaTimePointCloud = 0;
}
}
// Send image update
_deltaTimeImage += Time.unscaledDeltaTime;
if (_sendImage && _deltaTimeImage >= 1.0)
{
var png = ArUtils.GetCameraImage(ARInterface.GetInterface());
if (png != null)
{
SendData("image", png);
_deltaTimeImage = 0;
}
}
}
private void AnnotateButtonClick()
{
Debug.Log("attempting raycast");
var castPosition = FocusDot.transform.position;
TrackableHit hit;
// if (ARInterface.GetInterface().TryRaycast(castPosition.x, castPosition.y, ref anchorPoint))
if (Frame.Raycast(castPosition.x, castPosition.y, TrackableHitFlags.FeaturePoint, out hit))
{
Debug.Log("raycast success");
// var obj = GameObject.CreatePrimitive(PrimitiveType.Sphere);
// obj.transform.localScale = new Vector3(0.03f, 0.03f, 0.03f);
// obj.transform.position = anchorPoint.position;
// obj.transform.rotation = anchorPoint.rotation;
// anchors.Add(anchorPoint.id, obj.transform);
var png = ArUtils.GetCameraImage(ARInterface.GetInterface());
if (png != null)
{
if (ARInterface.GetInterface().TryGetPose(ref _pose))
{
var anchor = hit.Trackable.CreateAnchor(hit.Pose);
var annotation = new TrackedAnnotation(png, Camera.main.aspect, _pose, anchor);
_annotations.Add(annotation);
annotation.Show();
ObjToAnnotation.Add(annotation._obj, annotation);
setCurrentAnnotation(annotation);
// SocketConnection.Instance.SendData("annotate", annotation.ToJson());
}
}
}
else
{
Debug.Log("raycast unsuccessful");
}
}
// Update is called once per frame
void Update()
{
if (Time.time > _lastUpdate + 0.1)
{
_lastUpdate = Time.time;
if (ARInterface.GetInterface().TryGetPointCloud(ref m_PointCloud))
{
if (m_PointCloud.points.Count > 10)
{
var vertices = m_PointCloud.points.ToArray();
// var indices = ConvexHull.Generate(vertices);
// Console.WriteLine("indices: " + indices.Length);
m_Mesh.Clear();
m_Mesh.vertices = vertices;
// m_Mesh.triangles = indices;
}
// var numParticles = Mathf.Min(m_PointCloud.points.Count, m_MaxPointsToShow);
// if (m_Particles == null || m_Particles.Length != numParticles)
// m_Particles = new ParticleSystem.Particle[numParticles];
//
// for (int i = 0; i < numParticles; ++i)
// {
// m_Particles[i].position = m_PointCloud.points[i];
// m_Particles[i].startColor = new Color(1.0f, 1.0f, 1.0f);
// m_Particles[i].startSize = m_ParticleSize;
// }
//
// m_ParticleSystem.SetParticles(m_Particles, numParticles);
}
else
{
// m_Mesh.Clear();
// m_ParticleSystem.SetParticles(m_NoParticles, 1);
}
}
}
protected virtual void SetupARInterface()
{
m_ARInterface = ARInterface.GetInterface();
}
private void Update()
{
// Update camera matrix
var matrixTrs = Matrix4x4.TRS(Camera.main.transform.position, Camera.main.transform.rotation, new Vector3(1, 1, 1));
var matrixCombined = Camera.main.nonJitteredProjectionMatrix * matrixTrs.inverse;
LoadMatrix(CameraMatrix, matrixCombined);
// Update point cloud
timeSincePointCloudUpdate += Time.deltaTime;
if (timeSincePointCloudUpdate > 0)
{
if (ARInterface.GetInterface().TryGetPointCloud(ref m_PointCloud))
{
timeSincePointCloudUpdate = 0;
// testing reset
nextParticle = 0;
for (int i = 0; i < m_PointCloud.points.Count; ++i)
{
if (m_ParticleSystem != null)
{
m_Particles[nextParticle].position = m_PointCloud.points[i];
m_Particles[nextParticle].startColor = new Color(1.0f, 1.0f, 1.0f);
m_Particles[nextParticle].startSize = m_ParticleSize;
}
var p = m_PointCloud.points[i];
ParticleMatrix.SetColumn(nextParticle, new[] { p[0], p[1], p[2], 1 });
nextParticle += 1;
// testing reset
if (nextParticle == MAX_PARTICLES)
{
break;
}
nextParticle %= MAX_PARTICLES;
}
if (m_ParticleSystem != null)
{
m_ParticleSystem.SetParticles(m_Particles, m_Particles.Length);
}
}
}
// Reset buffers
for (int i = 0; i < MeshSize; i++)
{
for (int j = 0; j < MeshSize; j++)
{
_verticesZ[i, j] = 0;
_verticesWeight[i, j] = 0;
}
}
// Compute z distances
// var result = CameraMatrix * ParticleMatrix;
// foreach (var vec in result.EnumerateColumns())
// {
// var zCam = -vec[3];
// if (zCam < 0.1 || zCam > 100)
// {
// continue;
// }
//
// var x = vec[0] / vec[3];
// var y = vec[1] / vec[3];
//// var z = vec[2] / vec[3];
//
// // if within range
// if (x > -1 && x < 1 && y > -1 && y < 1)
// {
// for (int i = 0; i < MeshSize; i++)
// {
// for (int j = 0; j < MeshSize; j++)
// {
// var u = (double) 2 * i / MeshSize - 1;
// var v = (double) 2 * j / MeshSize - 1;
// var distSq = Math.Pow(x - u, 2) + Math.Pow(y - v, 2);
// var sigmaSq = Math.Pow(0.1, 2);
// var weight = 1.0 / Math.Sqrt(2 * Math.PI * sigmaSq) * Math.Exp(-distSq / (2 * sigmaSq));
//
// _verticesZ[i, j] += zCam * weight;
// _verticesWeight[i, j] += weight;
// }
// }
// }
// }
for (var idx = 0; idx < nextParticle; idx++)
{
var vec = ParticleMatrix.Column(idx);
var pos = Camera.main.WorldToScreenPoint(new Vector3(vec[0], vec[1], vec[2]));
if (pos.z < 0.1 || pos.z > 100)
{
continue;
}
// if within range
if (true)
{
for (int i = 0; i < MeshSize; i++)
{
for (int j = 0; j < MeshSize; j++)
{
var u = (double)i / MeshSize * Camera.main.pixelWidth;
var v = (double)j / MeshSize * Camera.main.pixelHeight;
var distSq = Math.Pow(pos.x - u, 2) + Math.Pow(pos.y - v, 2);
var sigmaSq = Math.Pow(10, 2);
var weight = 1.0 / Math.Sqrt(2 * Math.PI * sigmaSq) * Math.Exp(-distSq / (2 * sigmaSq));
_verticesZ[i, j] += pos.z * weight;
_verticesWeight[i, j] += weight;
}
}
}
}
// Recalculate mesh points
for (int i = 0; i < MeshSize; i++)
{
for (int j = 0; j < MeshSize; j++)
{
// var u = (double) 2 * i / MeshSize - 1;
// var v = (double) 2 * j / MeshSize - 1;
// var z = _verticesZ[i, j] / _verticesWeight[i, j];
// var w = 1;
// var vec = Vector<float>.Build.DenseOfArray(new[] {(float) (u * w), (float) (v * w), (float) (0 * w), (float) w});
// vec = CameraMatrix.Inverse() * vec;
// _vertices[_verticesIdx[i, j]] = new Vector3(vec[0] / vec[3], vec[1] / vec[3], vec[2] / vec[3]);
var u = (double)i / MeshSize * Camera.main.pixelWidth;
var v = (double)j / MeshSize * Camera.main.pixelHeight;
var z = _verticesZ[i, j] / _verticesWeight[i, j];
_vertices[_verticesIdx[i, j]] = Camera.main.ScreenToWorldPoint(new Vector3((float)u, (float)v, (float)z));
}
}
// Set new mesh
var mesh = _surface.GetComponent <MeshFilter>().mesh;
mesh.SetVertices(_vertices.ToList());
// mesh.triangles = mesh.triangles;
mesh.RecalculateNormals();
}