// Update is called once per frame
void Update()
{
if (runCalibration)
{
if (Input.GetMouseButton(0) || Input.GetMouseButton(1) || Input.GetMouseButton(2))
{
if (Input.GetMouseButton(0))
{
//Debug.Log(Input.mousePosition);
GameObject bc = GameObject.FindGameObjectWithTag("BlueCross");
bc.transform.localPosition = new Vector3(Map(Input.mousePosition.x, Screen.width / 2.0f - 320.0f, Screen.width / 2.0f + 320.0f, 0.0f, 640.0f) - 320.0f, -Map(Input.mousePosition.y, Screen.height / 2.0f + 240.0f, Screen.height / 2.0f - 240.0f, 0.0f, 480.0f) + 240.0f, 0.0f);
}
else if (Input.GetMouseButton(1))
{
GameObject yc = GameObject.FindGameObjectWithTag("YellowCross");
yc.transform.localPosition = new Vector3(Map(Input.mousePosition.x, Screen.width / 2.0f - 320.0f, Screen.width / 2.0f + 320.0f, 0.0f, 640.0f) - 320.0f, -Map(Input.mousePosition.y, Screen.height / 2.0f + 240.0f, Screen.height / 2.0f - 240.0f, 0.0f, 480.0f) + 240.0f, 0.0f);
nextBt = true;
}
else if (Input.GetMouseButton(2) && nextBt == true)
{
if (addKinectPoint())
{
addProjectorPoint();
Debug.Log("Point Added! -> (" + kinectCoordinates.Count + ") ");
nextBt = false;
}
else
{
Debug.Log("Kinect Point out of bounds!");
}
}
}
if (Input.GetKeyDown(KeyCode.A))
{
//PointerEventData pointer = new PointerEventData(EventSystem.current);
//pointer.position = Input.mousePosition;
//List<RaycastResult> raycastResults = new List<RaycastResult>();
//EventSystem.current.RaycastAll(pointer, raycastResults);
if (addKinectPoint())
{
addProjectorPoint();
Debug.Log("Point Added! -> " + kinectCoordinates.Count);
}
else
{
Debug.Log("Kinect Point out of bounds!");
}
}
if (Input.GetKeyDown(KeyCode.S))
{
if (kinectCoordinates.Count >= 8)
{
Debug.Log("Starting Calibration...");
findTransformation(kinectCoordinates, projectorCoordinates);
foundResult = true;
}
else
{
Debug.Log("Not Enough Points!");
}
}
if (Input.GetKeyDown(KeyCode.D) && foundResult == true)
{
showResult = !showResult;
if (!showResult)
{
screenTx.SetPixels32(resetPixels);
screenTx.Apply(false);
}
Debug.Log("Show result toggle: " + showResult);
}
if (Input.GetKeyDown(KeyCode.F) && foundResult == true)
{
using (CvFileStorage fs = new CvFileStorage("KinectCalibration.xml", null, FileStorageMode.Write))
{
string nodeName = "calibResult";
fs.Write(nodeName, result.ToCvMat());
nodeName = "kinectPoints";
Mat kinectPts = new Mat(1, kinectCoordinates.Count, MatType.CV_64FC3);
for (int i = 0; i < kinectCoordinates.Count; i++)
{
kinectPts.Set<CvPoint3D64f>(0, i, (CvPoint3D64f)kinectCoordinates[i]);
}
fs.Write(nodeName, kinectPts.ToCvMat());
nodeName = "projectorPoints";
Mat projPts = new Mat(1, projectorCoordinates.Count, MatType.CV_64FC2);
for (int i = 0; i < projectorCoordinates.Count; i++)
{
projPts.Set<CvPoint2D64f>(0, i, (CvPoint2D64f)projectorCoordinates[i]);
}
fs.Write(nodeName, projPts.ToCvMat());
fs.Dispose();
}
Debug.Log("Calib Data saved!");
}
if (Input.GetKeyDown(KeyCode.Q))
{
delLastPoints();
}
if (kinect.GetDepthRaw())
{
try
{
Mat src = DoDepthBuffer(kinect.usersDepthMap, KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight());
dBuffer = src.Clone();
src.ConvertTo(src, OpenCvSharp.CPlusPlus.MatType.CV_8UC1, 255.0f / NUI_IMAGE_DEPTH_MAXIMUM);
Mat show = new Mat(KinectWrapper.GetDepthHeight(), KinectWrapper.GetDepthWidth(), OpenCvSharp.CPlusPlus.MatType.CV_8UC4);
Mat alpha = new Mat(KinectWrapper.GetDepthHeight(), KinectWrapper.GetDepthWidth(), OpenCvSharp.CPlusPlus.MatType.CV_8UC1, new Scalar(255));
Mat[] planes = new Mat[4] { src, src, src, alpha };
Cv2.Merge(planes, show);
//Mat falseColorsMap = new Mat();
//Cv2.ApplyColorMap(src, falseColorsMap, OpenCvSharp.CPlusPlus.ColorMapMode.Rainbow);
//Cv2.ImShow("show", falseColorsMap);
int matSize = (int)show.Total() * show.Channels();
byte[] rColors = new byte[matSize];
Marshal.Copy(show.DataStart, rColors, 0, matSize);
scTex.LoadRawTextureData(rColors);
scTex.Apply(false);
ScreenObject.GetComponent<RawImage>().texture = scTex;
if (showResult)
{
//ResultObject.SetActive(true);
screenTx.SetPixels32(resetPixels);
long discarded = 0;
long drawn = 0;
long bounds = 0;
//Color32[] txcl = (Color32[])resetPixels.Clone();
Color32[] txcl = new Color32[screenTx.height * screenTx.width];
for (int i = 0; i < txcl.Length; i++)
{
Color32 cCol = new Color32(0, 0, 0, 255);
txcl[i] = cCol;
}
screenTx.SetPixels32(txcl, 0);
Color32 sccolor = Color.white;
for (int i = 0; i < show.Rows; i += 5)
{
for (int j = 0; j < show.Cols; j += 5)
{
CvPoint3D64f realVal = NuiTransformDepthImageToSkeleton((long)j, (long)i, dBuffer.Get<ushort>((int)i, (int)j));
if (realVal.Z < projThresh && realVal.Z > 1.0)
{
CvPoint2D64f scCoord = convertKinectToProjector(realVal);
if (scCoord.X > 0.0 && scCoord.X < Screen.width && scCoord.Y > 0.0 && scCoord.Y < Screen.height)
{
//Debug.Log(scCoord.X.ToString() + " " + scCoord.Y.ToString());
//Vec4b bgrPixel = falseColorsMap.At<Vec4b>(i, j);
//Color32 sccolor = new Color32(bgrPixel[2], bgrPixel[1], bgrPixel[0], 255);
int X = Mathf.CeilToInt((float)scCoord.X);
int Y = Mathf.CeilToInt((float)scCoord.Y);
int arrPos = ((screenTx.height - Y) * screenTx.width) + X;
//Debug.Log(scCoord.X + " -> " + X + " --" + scCoord.Y + " -> " + Y + " = " + arrPos + " == " + screenTx.height + " == " + screenTx.width);
txcl[arrPos] = sccolor;
//screenTx.SetPixel((int)scCoord.X, Screen.height - (int)scCoord.Y, sccolor);
drawn++;
}
else
{
bounds++;
}
}
else
{
discarded++;
}
}
}
Debug.Log("Discarded: " + discarded + " Bounds: " + bounds + " Drawn: " + drawn);
screenTx.SetPixels32(txcl, 0);
screenTx.Apply(false);
//GameObject.FindGameObjectWithTag("Restex").GetComponent<RawImage>().texture = screenTx;
//CvContour contourfinder = new CvContour();
}
else
{
//ResultObject.SetActive(false);
}
}
catch (System.Exception e)
{
throw e;
}
}
}
}
// Use this for initialization
void Start()
{
if (runCalibration)
{
Application.RegisterLogCallback(HandleLog);
scTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight(), TextureFormat.RGBA32, false);
screenTx = new Texture2D(Screen.width, Screen.height, TextureFormat.RGBA32, false);
resetPixels = new Color32[Screen.width * Screen.height];
for (int i = 0; i < resetPixels.Length; i++)
{
Color32 cCol = new Color32(0, 0, 0, 0);
resetPixels[i] = cCol;
}
screenTx.SetPixels32(resetPixels);
screenTx.Apply(false);
GameObject.FindGameObjectWithTag("Restex").GetComponent<RawImage>().texture = screenTx;
}
foundCoordinatesMatrix = new Mat();
rightSideMatrix = new Mat();
//result = new Mat(11,1,OpenCvSharp.CPlusPlus.MatType.CV_64FC1);
using (CvFileStorage fs = new CvFileStorage("KinectCalibration.xml", null, FileStorageMode.Read))
{
string nodeName = "projectorPoints";
CvFileNode param = fs.GetFileNodeByName(null, nodeName);
Mat pPts = new Mat(fs.Read<CvMat>(param), true);
for (int i = 0; i < pPts.Cols; i++)
{
projectorCoordinates.Add(pPts.Get<CvPoint2D64f>(0, i));
}
nodeName = "kinectPoints";
param = fs.GetFileNodeByName(null, nodeName);
Mat kPts = new Mat(fs.Read<CvMat>(param), true);
for (int i = 0; i < pPts.Cols; i++)
{
kinectCoordinates.Add(kPts.Get<CvPoint3D64f>(0, i));
}
nodeName = "calibResult";
param = fs.GetFileNodeByName(null, nodeName);
result = new Mat(fs.Read<CvMat>(param), true);
fs.Dispose();
}
if (result.Rows > 1)
{
foundResult = true;
Debug.Log("Calib Loaded: " + result.Get<double>(0, 0).ToString() + " " + result.Get<double>(0, 1) + " " + result.Get<double>(0, 2) + " " + result.Get<double>(0, 3) + " " + result.Get<double>(0, 4) + " " + result.Get<double>(0, 5) + " " + result.Get<double>(0, 6) + " " + result.Get<double>(0, 7) + " " + result.Get<double>(0, 8) + " " + result.Get<double>(0, 9) + " " + result.Get<double>(0, 10) + "--" + kinectCoordinates.Count + " " + projectorCoordinates.Count);
}
Debug.Log(Screen.width + " " + Screen.height);
}
