private void Update()
{
if (webcamTexture != null && webcamTexture.didUpdateThisFrame)
{
// Update new data from webcam device
UpdateWebcamBegin();
// Detect & Draw chessboard corners
bool detected = FindAndDrawChessboardCorners(webcamGrayMat, webcamModifiedMat);
if (detected == true && Input.GetKey(KeyCode.D))
{
// If detected, update camera transform accordingly
CamExtrinsicParam camParams = GetExtrinsicParameters();
UpdateCameraTransform(camParams);
}
// Apply modified data to webcam (image with corners detected)
UpdateWebcamEnd();
}
if (displayTexture != null)
{
canvasImageDisplay.texture = displayTexture;
}
}
private CamExtrinsicParam GetExtrinsicParameters()
{
// Compute camera extrinsic parameters : rotation & translation matrices.
Mat rotationMat = new Mat(3, 1, DepthType.Cv64F, 1);
Mat translationMat = new Mat(3, 1, DepthType.Cv64F, 1);
bool res = CvInvoke.SolvePnP(virtualCorners, imageCorners, intrinsicMatrix, distortionMatrix, rotationMat, translationMat);
if (res == false)
{
Debug.Log("SolvePnP Failed!");
return(null);
}
// Convert the rotation from 3 axis rotations into a rotation matrix.
double[] tempRotationData = new double[3];
Marshal.Copy(rotationMat.DataPointer, tempRotationData, 0, rotationMat.Width * rotationMat.Height);
tempRotationData[0] = tempRotationData[0];
tempRotationData[1] = -tempRotationData[1];
tempRotationData[2] = -tempRotationData[2];
GCHandle tempHandle = GCHandle.Alloc(tempRotationData, GCHandleType.Pinned);
// Final rotation matrix with Rodrigues function.
rotationMat = new Mat(3, 1, DepthType.Cv64F, 1, tempHandle.AddrOfPinnedObject(), sizeof(double));
Mat finalRotationMat = new Mat(3, 3, DepthType.Cv64F, 1);
CvInvoke.Rodrigues(rotationMat, finalRotationMat);
// Get all data to set Camera transform.
double[] rotationData = new double[9];
Marshal.Copy(finalRotationMat.DataPointer, rotationData, 0, finalRotationMat.Width * finalRotationMat.Height);
double[] translationData = new double[3];
Marshal.Copy(translationMat.DataPointer, translationData, 0, translationMat.Width * translationMat.Height);
// Return data.
CamExtrinsicParam ret = new CamExtrinsicParam();
ret.rotationData = rotationData;
ret.translationData = translationData;
return(ret);
}
private void UpdateCameraTransform(CamExtrinsicParam camParams)
{
double[] extrinsicRotation = camParams.rotationData;
double[] extrinsicTranslat = camParams.translationData;
// Projection matrix
Matrix4x4 projection = new Matrix4x4();
projection.m00 = (float)(2 * intrinsicMatrix[0, 0] / webcamTexture.width);
projection.m10 = 0;
projection.m20 = 0;
projection.m30 = 0;
projection.m01 = 0;
projection.m11 = (float)(2 * intrinsicMatrix[1, 1] / webcamTexture.height);
projection.m21 = 0;
projection.m31 = 0;
projection.m02 = (float)(1 - 2 * intrinsicMatrix[0, 2] / webcamTexture.width);
projection.m12 = (float)(-1 + (2 * intrinsicMatrix[1, 2] + 2) / webcamTexture.height);
projection.m22 = (Camera.main.nearClipPlane + Camera.main.farClipPlane) / (Camera.main.nearClipPlane - Camera.main.farClipPlane);
projection.m32 = -1;
projection.m03 = 0;
projection.m13 = 0;
projection.m23 = 2 * (Camera.main.nearClipPlane * Camera.main.farClipPlane) / (Camera.main.nearClipPlane - Camera.main.farClipPlane);
projection.m33 = 0;
Camera.main.projectionMatrix = projection;
Camera.main.fieldOfView = Mathf.Atan(1.0f / projection.m11) * 2.0f * Mathf.Rad2Deg;
Camera.main.aspect = webcamTexture.width / webcamTexture.height;
// Model view matrix
Matrix4x4 modelViewMatrix = new Matrix4x4();
modelViewMatrix.m00 = (float)extrinsicRotation[0];
modelViewMatrix.m10 = (float)extrinsicRotation[3];
modelViewMatrix.m20 = (float)extrinsicRotation[6];
modelViewMatrix.m30 = 0;
modelViewMatrix.m01 = (float)extrinsicRotation[1];
modelViewMatrix.m11 = (float)extrinsicRotation[4];
modelViewMatrix.m21 = (float)extrinsicRotation[7];
modelViewMatrix.m31 = 0;
modelViewMatrix.m02 = (float)extrinsicRotation[2];
modelViewMatrix.m12 = (float)extrinsicRotation[5];
modelViewMatrix.m22 = (float)extrinsicRotation[8];
modelViewMatrix.m32 = 0;
modelViewMatrix.m03 = (float)extrinsicTranslat[0];
modelViewMatrix.m13 = (float)extrinsicTranslat[1];
modelViewMatrix.m23 = (float)extrinsicTranslat[2];
modelViewMatrix.m33 = 1;
modelViewMatrix = modelViewMatrix.inverse;
// Update unity camera transform
Camera.main.transform.position = GetPositionFromModelView(modelViewMatrix);
Camera.main.transform.rotation = GetRotationFromModelView(modelViewMatrix);
}
