void Update()
{
UpdateViewAngles();
if (m_webCamTexture.isPlaying)
{
Color32[] colors = m_webCamTexture.GetPixels32();
byte[] gray_colors = new byte[colors.Length];
for (int i = 0; i < colors.Length; ++i)
{
gray_colors[i] = (byte)((colors[i].r + colors[i].g + colors[i].b) / 3);
}
TrackingState currentTrackingState = SonarLib.ProcessFrame(gray_colors, m_webCamTexture.width, m_webCamTexture.height);
UpdatePose(currentTrackingState);
}
}
public void ReinitalizeCamera(float horizontalViewAngle)
{
if (m_camera == null)
{
Debug.LogError("Camera component is found.");
return;
}
m_camera.fieldOfView = Camera.HorizontalToVerticalFieldOfView(horizontalViewAngle, m_camera.aspect);
m_focalLength = 1.0f / (2.0f * Mathf.Tan(horizontalViewAngle * Mathf.Deg2Rad * 0.5f));
m_cameraImageTransform = cameraImage.GetComponent <RectTransform>();
float pixelFocalLength = (float)(m_webCamTexture.width) * m_focalLength;
bool successInitialization = SonarLib.InitializeTrackingSystemForPinhole(1, m_webCamTexture.width, m_webCamTexture.height,
new Vector2(pixelFocalLength, pixelFocalLength),
new Vector2(m_webCamTexture.width * 0.5f, m_webCamTexture.height * 0.5f));
if (!successInitialization)
{
Debug.LogError("Sonar reinitialization fail.");
}
}
public static bool GetCameraWorldPose(out Quaternion q, out Vector3 position)
{
float[] rotationMatrix_data = new float[9];
float[] position_data = new float[3];
bool successFlag = false;
unsafe
{
fixed(float *r_ptr = &rotationMatrix_data[0])
{
fixed(float *p_ptr = &position_data[0])
{
successFlag = SonarLib.sonar_get_camera_world_pose((System.IntPtr)r_ptr, (System.IntPtr)p_ptr);
}
}
}
q = QuaternionFromMatrix(rotationMatrix_data);
position = new Vector3(position_data[0], position_data[1], position_data[2]);
return(successFlag);
}
void Start()
{
m_webCamTexture = new WebCamTexture(WebCamTexture.devices[0].name, 640, 480);
cameraImage.texture = m_webCamTexture;
m_webCamTexture.Play();
m_camera = GetComponent <Camera>();
m_cameraImageTransform = cameraImage.GetComponent <RectTransform>();
m_focalLength = (float)(m_webCamTexture.width);
if (m_camera == null)
{
Debug.LogError("Camera component is found.");
}
else
{
#if UNITY_ANDROID
m_camera.fieldOfView = Camera.HorizontalToVerticalFieldOfView(65, m_camera.aspect);;
#endif
float horizontalFieldOfView = Camera.VerticalToHorizontalFieldOfView(m_camera.fieldOfView, m_camera.aspect);
m_focalLength = 1.0f / (2.0f * Mathf.Tan(horizontalFieldOfView * Mathf.Deg2Rad * 0.5f));
}
m_cameraImageTransform = cameraImage.GetComponent <RectTransform>();
float pixelFocalLength = (float)(m_webCamTexture.width) * m_focalLength;
bool successInitialization = SonarLib.InitializeTrackingSystemForPinhole(1, m_webCamTexture.width, m_webCamTexture.height,
new Vector2(pixelFocalLength, pixelFocalLength),
new Vector2(m_webCamTexture.width * 0.5f, m_webCamTexture.height * 0.5f));
if (!successInitialization)
{
Debug.LogError("Sonar initialization fail.");
}
/*var cameraImageTransform = cameraImage.GetComponent<RectTransform>();
* if (webCamTexture.width > webCamTexture.height)
* {
* cameraImageTransform.localScale = new Vector3(1.0f, (float)(webCamTexture.width) / (float)(webCamTexture.height), 1.0f);
* }
* else
* {
* cameraImageTransform.localScale = new Vector3((float)(webCamTexture.height) / (float)(webCamTexture.width), 1.0f, 1.0f);
* }*/
}
void UpdatePose(TrackingState trackingState)
{
if (trackingState == TrackingState.Tracking)
{
Quaternion q;
Vector3 position;
SonarLib.GetCameraWorldPose(out q, out position);
if (lastTrackingsState == TrackingState.Tracking)
{
q = Quaternion.Slerp(q, lastQ, rotationSmooth);
position = Vector3.Slerp(position, lastPosition, positionSmooth);
lastQ = q;
lastPosition = position;
}
else
{
q = lastQ;
position = lastPosition;
}
transform.localRotation = q;
transform.localPosition = position;
}
lastTrackingsState = trackingState;
}
