bool DetectAndRaycastCirclePatternOntoChessboardPlane()
{
// Find circle pattern in camera image
Core.bitwise_not(_camTexGrayUndistortMat, _camTexGrayUndistortInvMat); // Invert. We need dark circles on a bright background.
if (!TrackingToolsHelper.FindAsymmetricCirclesGrid(_camTexGrayUndistortInvMat, _circlePatternSize, ref _circlePointsCameraImageMat))
{
return(false);
}
// Draw deteted circles.
TrackingToolsHelper.DrawFoundPattern(_camTexGrayUndistortMat, _circlePatternSize, _circlePointsCameraImageMat);
// Raycast circles against chessboard plane.
_calibrationboardPlane.SetNormalAndPosition(_calibrationBoardTransform.forward, _calibrationBoardTransform.position);
for (int p = 0; p < _circlePatternPointCount; p++)
{
// Tranform from points detected by camera to points in Unity world space.
Vector2 cameraImagePoint = _circlePointsCameraImageMat.ReadVector2(p);
cameraImagePoint.y = _cameraTexture.height - cameraImagePoint.y; // Unity screen space has zero at bottom-left, OpenCV textures has zero at top-left.
Ray ray = _mainCamera.ScreenPointToRay(cameraImagePoint);
float hitDistance;
if (!_calibrationboardPlane.Raycast(ray, out hitDistance))
{
return(false);
}
// For extrinsics calibration (using stereoCalibrate()).
Vector3 worldPoint = ray.origin + ray.direction * hitDistance;
_circlePointsDetectedWorldMat.WriteVector3(worldPoint, p);
// For intrinsics calibration (using calibrateCalmera()).
Vector3 realModelPoint = Quaternion.Inverse(_calibrationBoardTransform.rotation) * (worldPoint - _calibrationBoardTransform.position);
realModelPoint.z = 0; // Remove very small numbers. It seems CalibrateCamera() does not accept varying z values. I got an execption.
realModelPoint *= 1000; // To millimeters
_circlePointsRealModelMat.WriteVector3(realModelPoint, p);
}
return(true);
}