Beispiel #1
0
        public override void PreparePose()
        {
            var  poses   = ArMarkers.GetAll().Where(m => IsArMarkerValid(m));
            bool hadPose = _hasPose;

            _hasPose = poses.Count() > 0;

            if (!_hasPose)
            {
                return;
            }

            // weighted average - by marker confidence
            float x = 0, y = 0, z = 0;
            float qx = 0, qy = 0, qz = 0, qw = 0;
            float totalConfidence = 0;

            foreach (var pose in poses)
            {
                var conf = pose.Confidence;
                x += conf * pose.DetectedCameraPosition.x;
                y += conf * pose.DetectedCameraPosition.y;
                z += conf * pose.DetectedCameraPosition.z;

                qx += pose.DetectedCameraRotation.x;
                qy += pose.DetectedCameraRotation.y;
                qz += pose.DetectedCameraRotation.z;
                qw += pose.DetectedCameraRotation.w;

                totalConfidence += conf;
            }

            float k               = 1.0f / Mathf.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
            var   rot             = new Quaternion(qx * k, qy * k, qz * k, qw * k);
            var   pos             = new Vector3(x, y, z) / totalConfidence;

            if (UseAverages && hadPose)
            {
                _rotation = Quaternion.Slerp(_rotation, rot, AverageWeight);
                _position = AverageWeight * _position + (1 - AverageWeight) * pos;
            }
            else
            {
                _position = pos;
                _rotation = rot;
            }
        }
Beispiel #2
0
        private Vector3 GetMarkerWorldPosition(int id)
        {
            var marker = ArMarkers.Get(id);

            //var marker = _markers.FirstOrDefault((m) => m.id == id);
            //var surface = SurfaceManager.Instance.Get(SurfaceName);

            if (marker == null)
            {
                // should never happen, in theory
                Debug.Log("Unable to find marker " + id);
                return(Vector3.zero);
            }

            return(marker.transform.position);

            //var worldOffsetFromTopLeft = marker.GetUnityPosition(surface);
            //var localOffsetFromTopLeft = surface.Rotation * worldOffsetFromTopLeft;

            //return surface.GetCornerPosition(Corner.TopLeft) + localOffsetFromTopLeft;
        }