public static void ReverseOrder(MatOfPoint2f points)
{
int count = points.rows();
for (int i = 0; i < count / 2; i++)
{
Vector2 vec2 = points.ReadVector2(i);
int i2 = count - i - 1;
points.WriteVector2(points.ReadVector2(i2), i);
points.WriteVector2(vec2, i2);
}
}
float ComputeAverageMovement()
{
float averageMovement = 0;
for (int n = 0; n < _chessPatternPointCount; n++)
{
Vector2 chessCorner = _chessCornersImageMat.ReadVector2(n);
if (_successFrameCount > 0)
{
averageMovement += (_prevChessCorners[n] - chessCorner).magnitude;
}
_prevChessCorners[n] = chessCorner;
}
if (_successFrameCount == 0)
{
return(0);
}
return(averageMovement / (float)_prevChessCorners.Length / (float)_cameraTexture.height);
}
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);
}