/// <summary>
/// Detects the Aruco markers on the current images of the cameras and store the results in the <see cref="MarkerCornersCurrentImage"/> and
/// <see cref="MarkerIdsCurrentImage"/> properties.
/// </summary>
public virtual void DetectMarkers()
{
if (!IsConfigured)
{
throw new Exception("Configure the calibration controller before detect markers.");
}
for (int cameraId = 0; cameraId < ArucoCamera.CameraNumber; cameraId++)
{
Std.VectorInt markerIds;
Std.VectorVectorPoint2f markerCorners, rejectedCandidateCorners;
Cv.Mat image = ArucoCamera.Images[cameraId];
Aruco.DetectMarkers(image, CalibrationBoard.Dictionary, out markerCorners, out markerIds, DetectorParameters, out rejectedCandidateCorners);
MarkerCornersCurrentImage[cameraId] = markerCorners;
MarkerIdsCurrentImage[cameraId] = markerIds;
if (RefineMarkersDetection)
{
Aruco.RefineDetectedMarkers(image, CalibrationBoard.Board, MarkerCornersCurrentImage[cameraId], MarkerIdsCurrentImage[cameraId],
rejectedCandidateCorners);
}
}
}
/// <summary>
/// <see cref="ArucoObjectTracker.Detect(int, Dictionary, HashSet{ArucoObject})"/>
/// </summary>
public override void Detect(int cameraId, Aruco.Dictionary dictionary)
{
if (!IsActivated)
{
return;
}
CameraParameters cameraParameters = arucoTracker.ArucoCamera.CameraParameters;
Std.VectorVectorPoint2f diamondCorners = null;
Std.VectorVec4i diamondIds = null;
if (arucoTracker.MarkerTracker.DetectedMarkers[cameraId][dictionary] > 0)
{
if (cameraParameters == null)
{
Aruco.DetectCharucoDiamond(arucoTracker.ArucoCamera.Images[cameraId], arucoTracker.MarkerTracker.MarkerCorners[cameraId][dictionary],
arucoTracker.MarkerTracker.MarkerIds[cameraId][dictionary], DETECT_SQUARE_MARKER_LENGTH_RATE, out diamondCorners, out diamondIds);
}
else
{
Aruco.DetectCharucoDiamond(arucoTracker.ArucoCamera.Images[cameraId], arucoTracker.MarkerTracker.MarkerCorners[cameraId][dictionary],
arucoTracker.MarkerTracker.MarkerIds[cameraId][dictionary], DETECT_SQUARE_MARKER_LENGTH_RATE, out diamondCorners, out diamondIds,
cameraParameters.CamerasMatrix[cameraId], cameraParameters.DistCoeffs[cameraId]);
}
}
DiamondCorners[cameraId][dictionary] = diamondCorners;
DiamondIds[cameraId][dictionary] = diamondIds;
DetectedDiamonds[cameraId][dictionary] = (diamondIds != null) ? (int)diamondIds.Size() : 0;
}
/// <summary>
/// <see cref="ArucoObjectTracker.Draw(int, Dictionary)"/>
/// </summary>
public override void Draw(int cameraId, Aruco.Dictionary dictionary)
{
if (!IsActivated)
{
return;
}
bool updatedCameraImage = false;
Cv.Core.Mat[] cameraImages = arucoTracker.ArucoCamera.Images;
// Draw the detected markers
// TODO: draw only markers in ArucoObjects list + add option to draw all the detected markers
if (arucoTracker.DrawDetectedMarkers && DetectedMarkers[cameraId][dictionary] > 0)
{
Aruco.DrawDetectedMarkers(cameraImages[cameraId], MarkerCorners[cameraId][dictionary], MarkerIds[cameraId][dictionary]);
updatedCameraImage = true;
}
// Draw the rejected marker candidates
if (arucoTracker.DrawRejectedCandidates && RejectedCandidateCorners[cameraId][dictionary].Size() > 0)
{
Aruco.DrawDetectedMarkers(cameraImages[cameraId], RejectedCandidateCorners[cameraId][dictionary], REJECTED_MARKERS_CANDIDATES_COLOR);
updatedCameraImage = true;
}
if (updatedCameraImage)
{
arucoTracker.ArucoCamera.Images = cameraImages;
}
}
/// <summary>
/// <see cref="ArucoObjectTracker.Draw(int, Dictionary, HashSet{ArucoObject})"/>
/// </summary>
public override void Draw(int cameraId, Aruco.Dictionary dictionary)
{
if (!IsActivated || arucoTracker.MarkerTracker.DetectedMarkers[cameraId][dictionary] <= 0)
{
return;
}
bool updatedCameraImage = false;
Cv.Core.Mat[] cameraImages = arucoTracker.ArucoCamera.Images;
CameraParameters cameraParameters = arucoTracker.ArucoCamera.CameraParameters;
foreach (var arucoGridBoard in arucoTracker.GetArucoObjects <ArucoGridBoard>(dictionary))
{
if (arucoTracker.DrawAxes && cameraParameters != null && arucoGridBoard.MarkersUsedForEstimation > 0 && arucoGridBoard.Rvec != null)
{
Aruco.DrawAxis(cameraImages[cameraId], cameraParameters.CamerasMatrix[cameraId], cameraParameters.DistCoeffs[cameraId],
arucoGridBoard.Rvec, arucoGridBoard.Tvec, arucoGridBoard.AxisLength);
updatedCameraImage = true;
}
}
if (updatedCameraImage)
{
arucoTracker.ArucoCamera.Images = cameraImages;
}
}
public void Draw()
{
if (!IsConfigured)
{
return;
}
bool updatedCameraImage = false;
Cv.Core.Mat[] cameraImages = ArucoCamera.Images;
for (int cameraId = 0; cameraId < ArucoCamera.CamerasNumber; cameraId++)
{
if (MarkerIdsCurrentImage[cameraId] != null && MarkerIdsCurrentImage[cameraId].Size() > 0)
{
Aruco.DrawDetectedMarkers(cameraImages[cameraId], MarkerCornersCurrentImage[cameraId], MarkerIdsCurrentImage[cameraId]);
updatedCameraImage = true;
}
}
if (updatedCameraImage)
{
ArucoCamera.Images = cameraImages;
}
}
void Update()
{
// if space is down do AR tranformation
if (notCalibrated && Input.GetKeyDown(KeyCode.Space))
{
// laod camera calibrations setting
CameraCalibSerializable calidSaveData = Utilities.LoadCameraCalibrationParams();
// initialise marhsallers
MatDoubleMarshaller distCoeffs = new MatDoubleMarshaller(calidSaveData.distortionCoefficients);
MatDoubleMarshaller cameraMatrix = new MatDoubleMarshaller(calidSaveData.cameraMatrix);
double reProjectionError = calidSaveData.reProjectionError;
// get calibration data
calibData = new UCameraCalibrationData(distCoeffs, cameraMatrix, reProjectionError);
notCalibrated = false;
}
else if (!notCalibrated)
{
// estimate charuco board pose
(
UDetectMarkersData markerData,
UBoardMarkerPoseEstimationDataEuler poseEstimationData
) = Aruco.UEstimateCharucoBoardPose(
_webCamTexture.GetPixels32(),
_webCamTexture.width,
_webCamTexture.height,
boardParameters,
calibData.cameraMatrix.NativeDataPointer,
calibData.distCoeffs.NativeDataPointer
);
TransformGameObjects(poseEstimationData, markerData);
}
}
private void OnDetectionDone()
{
if (displayCameraPreview)
{
Imgproc.cvtColor(downScaleRgbaMat, rgbMat4preview, Imgproc.COLOR_RGBA2RGB);
if (ids.total() > 0)
{
Aruco.drawDetectedMarkers(rgbMat4preview, corners, ids, new Scalar(255, 0, 0));
for (int i = 0; i < ids.total(); i++)
{
Aruco.drawAxis(rgbMat4preview, camMatrix, distCoeffs, rvecs, tvecs, markerLength * 0.5f);
}
}
OpenCVForUnity.Utils.fastMatToTexture2D(rgbMat4preview, texture);
}
if (applyEstimationPose)
{
if (hasUpdatedARTransformMatrix)
{
hasUpdatedARTransformMatrix = false;
// Apply camera transform matrix.
ARM = arCamera.transform.localToWorldMatrix * ARM;
ARUtils.SetTransformFromMatrix(arGameObject.transform, ref ARM);
}
}
isDetecting = false;
}
public override void Draw(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image)
{
if (DetectedMarkers[cameraId][dictionary] > 0)
{
// Draw all the detected markers
if (arucoTracker.DrawDetectedMarkers)
{
// TODO: draw only markers in ArucoObjects list + add option to draw all the detected markers
Aruco.DrawDetectedMarkers(image, MarkerCorners[cameraId][dictionary], MarkerIds[cameraId][dictionary]);
}
// Draw axes of detected tracked markers
if (arucoTracker.DrawAxes && cameraParameters != null && MarkerRvecs[cameraId][dictionary] != null)
{
for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++)
{
ArucoObject foundArucoObject;
int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkerIds[cameraId][dictionary].At(i));
if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject))
{
Aruco.DrawAxis(image, cameraParameters.CameraMatrices[cameraId], cameraParameters.DistCoeffs[cameraId],
MarkerRvecs[cameraId][dictionary].At(i), MarkerTvecs[cameraId][dictionary].At(i), EstimatePoseMarkerLength);
}
}
}
}
// Draw the rejected marker candidates
if (arucoTracker.DrawRejectedCandidates && RejectedCandidateCorners[cameraId][dictionary].Size() > 0)
{
Aruco.DrawDetectedMarkers(image, RejectedCandidateCorners[cameraId][dictionary]);
}
}
public void MarkerProcessing(Mat MatImg)
{
if (MatImg != null)
{
Imgproc.cvtColor(MatImg, MatImg, Imgproc.COLOR_RGBA2RGB);
Aruco.detectMarkers(MatImg, dictionary, corners, ids, detectorParams, rejectedCorners, camMatrix, distCoeffs);
if (ids.total() > 0)
{
OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_int(ids, list_ids);
// draw markers.
Aruco.drawDetectedMarkers(MatImg, corners, ids, new Scalar(0, 255, 0));
// estimate pose.
if (applyEstimationPose)
{
EstimatePoseCanonicalMarker(MatImg);
}
if (showRejectedCorners && rejectedCorners.Count > 0)
{
Aruco.drawDetectedMarkers(MatImg, rejectedCorners, new Mat(), new Scalar(255, 0, 0));
}
}
}
}
// Methods
/// <summary>
/// Create the image and the image texture of the <see cref="ArucoObject"/>.
/// </summary>
public virtual void Create()
{
Cv.Mat image = null;
ImageTexture = null;
// In case of a marker
ArucoMarker marker = ArucoObject as ArucoMarker;
if (marker != null)
{
marker.Dictionary.DrawMarker(marker.MarkerId, (int)marker.MarkerSideLength, out image, marker.MarkerBorderBits);
}
// In case of a grid board
ArucoGridBoard arucoGridBoard = ArucoObject as ArucoGridBoard;
if (arucoGridBoard != null)
{
Aruco.GridBoard gridBoard = arucoGridBoard.Board as Aruco.GridBoard;
gridBoard.Draw(arucoGridBoard.ImageSize, out image, arucoGridBoard.MarginsSize, arucoGridBoard.MarkerBorderBits);
}
// In case of a charuco board
ArucoCharucoBoard arucoCharucoBoard = ArucoObject as ArucoCharucoBoard;
if (arucoCharucoBoard != null)
{
Aruco.CharucoBoard charucoBoard = arucoCharucoBoard.Board as Aruco.CharucoBoard;
charucoBoard.Draw(arucoCharucoBoard.ImageSize, out image, arucoCharucoBoard.MarginsSize, arucoCharucoBoard.MarkerBorderBits);
}
// In case of a diamond
ArucoDiamond diamond = ArucoObject as ArucoDiamond;
if (diamond != null && diamond.Ids.Length == 4)
{
Cv.Vec4i ids = new Cv.Vec4i();
for (int i = 0; i < diamond.Ids.Length; ++i)
{
ids.Set(i, diamond.Ids[i]);
}
Aruco.DrawCharucoDiamond(diamond.Dictionary, ids, (int)diamond.SquareSideLength, (int)diamond.MarkerSideLength, out image);
}
// Set the properties
Image = image;
if (Image != null)
{
// Vertical flip to correctly display the image on the texture
int verticalFlipCode = 0;
Cv.Mat imageForTexture = Image.Clone();
Cv.Flip(imageForTexture, imageForTexture, verticalFlipCode);
// Load the image to the texture
int markerDataSize = (int)(Image.ElemSize() * Image.Total());
ImageTexture = new Texture2D(Image.Cols, Image.Rows, TextureFormat.RGB24, false);
ImageTexture.LoadRawTextureData(imageForTexture.DataIntPtr, markerDataSize);
ImageTexture.Apply();
}
}
public override void Detect(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image)
{
base.Detect(cameraId, dictionary, image);
ArucoMarkerTracker markerTracker = arucoTracker.MarkerTracker;
Std.VectorVectorPoint2f diamondCorners = null;
Std.VectorVec4i diamondIds = null;
if (markerTracker.DetectedMarkers[cameraId][dictionary] > 0)
{
if (arucoCameraUndistortion == null)
{
Aruco.DetectCharucoDiamond(image, markerTracker.MarkerCorners[cameraId][dictionary], markerTracker.MarkerIds[cameraId][dictionary],
DetectSquareMarkerLengthRate, out diamondCorners, out diamondIds);
}
else
{
Aruco.DetectCharucoDiamond(image, markerTracker.MarkerCorners[cameraId][dictionary], markerTracker.MarkerIds[cameraId][dictionary],
DetectSquareMarkerLengthRate, out diamondCorners, out diamondIds, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId],
arucoCameraUndistortion.UndistortedDistCoeffs[cameraId]);
}
}
DiamondCorners[cameraId][dictionary] = diamondCorners;
DiamondIds[cameraId][dictionary] = diamondIds;
DetectedDiamonds[cameraId][dictionary] = (diamondIds != null) ? (int)diamondIds.Size() : 0;
}
public void Detect()
{
if (!IsConfigured)
{
return;
}
for (int cameraId = 0; cameraId < ArucoCamera.CamerasNumber; cameraId++)
{
Std.VectorInt markerIds;
Std.VectorVectorPoint2f markerCorners, rejectedCandidateCorners;
Cv.Core.Mat image = ArucoCamera.Images[cameraId];
Aruco.DetectMarkers(image, CalibrationBoard.Dictionary, out markerCorners, out markerIds, DetectorParameters, out rejectedCandidateCorners);
MarkerCornersCurrentImage[cameraId] = markerCorners;
MarkerIdsCurrentImage[cameraId] = markerIds;
if (RefineMarkersDetection)
{
Aruco.RefineDetectedMarkers(image, CalibrationBoard.Board, MarkerCornersCurrentImage[cameraId], MarkerIdsCurrentImage[cameraId], rejectedCandidateCorners);
}
}
}
public override void Draw(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image)
{
base.Draw(cameraId, dictionary, image);
if (DetectedMarkers[cameraId][dictionary] > 0)
{
// Draw all the detected markers
if (arucoTracker.DrawDetectedMarkers)
{
Aruco.DrawDetectedMarkers(image, MarkerCorners[cameraId][dictionary], MarkerIds[cameraId][dictionary]);
}
// Draw axes of detected tracked markers
if (arucoTracker.DrawAxes && arucoCameraUndistortion != null && MarkerRvecs[cameraId][dictionary] != null)
{
for (uint i = 0; i < DetectedMarkers[cameraId][dictionary]; i++)
{
ArucoObject foundArucoObject;
int detectedMarkerHashCode = ArucoMarker.GetArucoHashCode(MarkerIds[cameraId][dictionary].At(i));
if (arucoTracker.ArucoObjects[dictionary].TryGetValue(detectedMarkerHashCode, out foundArucoObject))
{
Aruco.DrawAxis(image, arucoCameraUndistortion.RectifiedCameraMatrices[cameraId], arucoCameraUndistortion.UndistortedDistCoeffs[cameraId],
MarkerRvecs[cameraId][dictionary].At(i), MarkerTvecs[cameraId][dictionary].At(i), estimatePoseMarkerLength);
}
}
}
}
// Draw the rejected marker candidates
if (arucoTracker.DrawRejectedCandidates && RejectedCandidateCorners[cameraId][dictionary].Size() > 0)
{
Aruco.DrawDetectedMarkers(image, RejectedCandidateCorners[cameraId][dictionary]);
}
}
// Update is called once per frame
void Update()
{
if (webCamTextureToMatHelper.IsPlaying() && webCamTextureToMatHelper.DidUpdateThisFrame())
{
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
Imgproc.cvtColor(rgbaMat, rgbMat, Imgproc.COLOR_RGBA2RGB);
foreach (MarkerSettings settings in markerSettingsList)
{
settings.setAllARGameObjectsDisable();
}
Aruco.detectMarkers(rgbMat, dictionaryAruco, cornersAruco, idsAruco);
if (idsAruco.total() > 0)
{
SetMarker();
}
else
{
SetMarkerLess();
}
Utils.fastMatToTexture2D(rgbaMat, texture);
}
}
// Use this for initialization
void Start()
{
GameObject cameraAR = GameObject.Find("ARCamera");
ARCamera = cameraAR.GetComponent <Camera>();
markerSettingsMarkerLessActual = null;
markerSettingsMarkerActual = null;
patternTrackingInfo = new PatternTrackingInfo();
markerSettingsList = markerList.transform.GetComponentsInChildren <MarkerSettings>();
if (markerSettingsList.Length == 0)
{
existeObjetoDetectar = false;
}
webCamTextureToMatHelper = gameObject.GetComponent <WebCamTextureToMatHelper>();
#if UNITY_ANDROID && !UNITY_EDITOR
// Avoids the front camera low light issue that occurs in only some Android devices (e.g. Google Pixel, Pixel2).
webCamTextureToMatHelper.avoidAndroidFrontCameraLowLightIssue = true;
#endif
webCamTextureToMatHelper.Initialize();
dictionaryAruco = Aruco.getPredefinedDictionary(PropertiesModel.DictionaryId);
cornersAruco = new List <Mat>();
idsAruco = new Mat();
}
// Methods
/// <summary>
/// Create the image and the image texture of the <see cref="ArucoObject"/>.
/// </summary>
public virtual void Create()
{
Cv.Core.Mat image = null;
ImageTexture = null;
// In case of a marker
ArucoMarker marker = ArucoObject as ArucoMarker;
if (marker != null)
{
marker.Dictionary.DrawMarker(marker.MarkerId, (int)marker.MarkerSideLength, out image, marker.MarkerBorderBits);
}
// In case of a grid board
ArucoGridBoard arucoGridBoard = ArucoObject as ArucoGridBoard;
if (arucoGridBoard != null)
{
Aruco.GridBoard gridBoard = arucoGridBoard.Board as Aruco.GridBoard;
gridBoard.Draw(arucoGridBoard.ImageSize, out image, arucoGridBoard.MarginsSize, arucoGridBoard.MarkerBorderBits);
}
// In case of a charuco board
ArucoCharucoBoard arucoCharucoBoard = ArucoObject as ArucoCharucoBoard;
if (arucoCharucoBoard != null)
{
Aruco.CharucoBoard charucoBoard = arucoCharucoBoard.Board as Aruco.CharucoBoard;
charucoBoard.Draw(arucoCharucoBoard.ImageSize, out image, arucoCharucoBoard.MarginsSize, arucoCharucoBoard.MarkerBorderBits);
}
// In case of a diamond
ArucoDiamond diamond = ArucoObject as ArucoDiamond;
if (diamond != null && diamond.Ids.Length == 4)
{
Cv.Core.Vec4i ids = new Cv.Core.Vec4i();
for (int i = 0; i < diamond.Ids.Length; ++i)
{
ids.Set(i, diamond.Ids[i]);
}
Aruco.DrawCharucoDiamond(diamond.Dictionary, ids, (int)diamond.SquareSideLength, (int)diamond.MarkerSideLength, out image);
}
// Vertical flip to convert the image from Unity's left-handed coordinate system to OpenCV's right-handed coordinate system
int verticalFlipCode = 0;
Cv.Core.Flip(image, image, verticalFlipCode);
// Set the properties
Image = image;
if (Image != null)
{
int markerDataSize = (int)(Image.ElemSize() * Image.Total());
ImageTexture = new Texture2D(Image.cols, Image.rows, TextureFormat.RGB24, false);
ImageTexture.LoadRawTextureData(Image.dataIntPtr, markerDataSize);
ImageTexture.Apply();
}
}
// MonoBehaviour methods
/// <summary>
/// Calls <see cref="UpdateProperties()"/>.
/// </summary>
protected virtual void Awake()
{
if (Dictionary == null)
{
dictionary = Aruco.GetPredefinedDictionary(dictionaryName);
}
UpdateProperties();
}
// MonoBehaviour methods
/// <summary>
/// Initializes the properties.
/// </summary>
protected virtual void Awake()
{
if (Dictionary == null)
{
dictionary = Aruco.GetPredefinedDictionary(dictionaryName);
}
UpdateArucoHashCode();
}
void ArucoDetection()
{
// Detect ArUco markers
Dictionary dict = Aruco.getPredefinedDictionary(Aruco.DICT_4X4_1000);
Aruco.detectMarkers(cached_initMat, dict, corners, ids);
Aruco.drawDetectedMarkers(cached_initMat, corners, ids);
// Debug.Log("AD - 93: Markers Detected");
// Debug.LogFormat("Corners: {0}", corners.Count);
// Get desired corner of marker
Point[] src_point_array = new Point[POINT_COUNT];
for (int i = 0; i < corners.Count; i++)
{
int aruco_id = (int)(ids.get(i, 0)[0]);
int src_i = arucoTosrc(aruco_id);
int corner_i = aruco_id % 4;
// Debug.LogFormat("AD - 101: aruco_id: {0}; corner_i: {1}; src_i: {2}", aruco_id, corner_i, src_i);
// Store corner[i] into spa[src_i]
src_point_array[src_i] = new Point(corners[i].get(0, corner_i)[0], corners[i].get(0, corner_i)[1]);
// Display the corner as circle on outMat.
Imgproc.circle(cached_initMat, src_point_array[src_i], 10, new Scalar(255, 255, 0));
}
// Converting to Ray values for Raycast
Camera _cam = Camera.main;
if (_cam != null)
{
for (int i = 0; i < POINT_COUNT; i++)
{
if (src_point_array[i] != null)
{
src_ray_array[i] = _cam.ScreenPointToRay(
new Vector3((float)src_point_array[i].x, (float)src_point_array[i].y, 0)).direction;
}
}
}
// Debug.LogFormat("Detected Direction: {0}", src_ray_array[0]);
// Debug.LogFormat("Camera Direction: {0}", _cam.transform.forward);
// Count non-null source points
bool spa_full = (count_src_nulls() == 7);
// Check if have valid faces
for (int i = 0; i < FACE_COUNT; i++)
{
// faceX_full[i] = check_faces(i);
faceX_full[i] = check_faces(i);
}
Core.flip(cached_initMat, outMat, 0);
}
void Update()
{
if (!runningOnAndroid)
{
if (notCalibrated && Input.GetKeyDown(KeyCode.Space))
{
bool sucess = Aruco.UFindCharucoBoardCorners(_webCamTexture.GetPixels32(), _webCamTexture.width, _webCamTexture.height, boardParameters, allCharucoIds, allCharucoCorners);
if (sucess)
{
numOfSuccessfulFrames++;
}
if (numOfSuccessfulFrames >= numOfFrames)
{
calibData = Aruco.UCalibrateCameraCharuco(_webCamTexture.width, _webCamTexture.height, boardParameters, allCharucoIds, allCharucoCorners);
CameraCalibSerializable calidSaveData;
calidSaveData.distortionCoefficients = (double[][])calibData.distCoeffs.GetMangedObject();
calidSaveData.cameraMatrix = (double[][])calibData.cameraMatrix.GetMangedObject();
calidSaveData.reProjectionError = calibData.reProjectionError;
Utilities.SaveCameraCalibrationParams(calidSaveData);
notCalibrated = false;
}
}
}
else
{
if (notCalibrated)
{
if (Input.touchCount > 0)
{
bool sucess = Aruco.UFindCharucoBoardCorners(_webCamTexture.GetPixels32(), _webCamTexture.width, _webCamTexture.height, boardParameters, allCharucoIds, allCharucoCorners);
if (sucess)
{
numOfSuccessfulFrames++;
}
if (numOfSuccessfulFrames >= numOfFrames)
{
calibData = Aruco.UCalibrateCameraCharuco(_webCamTexture.width, _webCamTexture.height, boardParameters, allCharucoIds, allCharucoCorners);
CameraCalibSerializable calidSaveData;
calidSaveData.distortionCoefficients = (double[][])calibData.distCoeffs.GetMangedObject();
calidSaveData.cameraMatrix = (double[][])calibData.cameraMatrix.GetMangedObject();
calidSaveData.reProjectionError = calibData.reProjectionError;
Utilities.SaveCameraCalibrationParams(calidSaveData);
notCalibrated = false;
}
}
}
}
}
private double CalibrateCameraAruco(List <List <Mat> > allCorners, List <Mat> allIds, CharucoBoard board, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List <Mat> rvecs = null, List <Mat> tvecs = null, int calibrationFlags = 0)
{
// prepare data for calibration
int nFrames = allCorners.Count;
int allLen = 0;
int[] markerCounterPerFrameArr = new int[allCorners.Count];
for (int i = 0; i < nFrames; ++i)
{
markerCounterPerFrameArr[i] = allCorners[i].Count;
allLen += allCorners[i].Count;
}
int[] allIdsConcatenatedArr = new int[allLen];
int index = 0;
for (int j = 0; j < allIds.Count; ++j)
{
int[] idsArr = new int[(int)allIds[j].total()];
allIds[j].get(0, 0, idsArr);
for (int k = 0; k < idsArr.Length; ++k)
{
allIdsConcatenatedArr[index + k] = (int)idsArr[k];
}
index += idsArr.Length;
}
using (Mat allIdsConcatenated = new Mat(1, allLen, CvType.CV_32SC1))
using (Mat markerCounterPerFrame = new Mat(1, nFrames, CvType.CV_32SC1))
{
List <Mat> allCornersConcatenated = new List <Mat>();
foreach (var c in allCorners)
{
foreach (var m in c)
{
allCornersConcatenated.Add(m);
}
}
allIdsConcatenated.put(0, 0, allIdsConcatenatedArr);
markerCounterPerFrame.put(0, 0, markerCounterPerFrameArr);
if (rvecs == null)
{
rvecs = new List <Mat>();
}
if (tvecs == null)
{
tvecs = new List <Mat>();
}
return(Aruco.calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, markerCounterPerFrame, board, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags));
}
}
public Vector4 DetectMarkers(Mat rgbMat)
{
Mat ids = new Mat();
List <Mat> corners = new List <Mat>();
List <Mat> rejectedCorners = new List <Mat>();
Mat rvecs = new Mat();
//Mat rotMat = new Mat(3, 3, CvType.CV_64FC1);
Mat tvecs = new Mat();
DetectorParameters detectorParams = DetectorParameters.create();
Dictionary dictionary = Aruco.getPredefinedDictionary((int)dictionaryId);
Vector4 ARM = new Vector3();
// detect markers
Aruco.detectMarkers(rgbMat, dictionary, corners, ids, detectorParams, rejectedCorners, camMatrix, distCoeffs);
// if at least one marker detected
if (ids.total() > 0)
{
// estimate pose, from marker to camera
Aruco.estimatePoseSingleMarkers(corners, markerSize, camMatrix, distCoeffs, rvecs, tvecs);
// Marker centre location
double[] tvecArr = tvecs.get(0, 0);
// image flip + coordinates transformation (OpenCV to Unity)
ARM = new Vector4((float)tvecArr[0] + 0.005f, (float)tvecArr[1] + 0.005f, -(float)tvecArr[2], 1f);
Debug.Log("raw ARM " + ARM.ToString("f5"));
//// Rotation and convert to Unity matrix format
//double[] rvecArr = rvecs.get(0, 0);
//Mat rvec = new Mat(3, 1, CvType.CV_64FC1);
//rvec.put(0, 0, rvecArr);
//Calib3d.Rodrigues(rvec, rotMat);
//double[] rotMatArr = new double[rotMat.total()];
//rotMat.get(0, 0, rotMatArr);
//// Transformation matrix
//Matrix4x4 transformationM = new Matrix4x4();
//transformationM.SetRow(0, new Vector4((float)rotMatArr[0], (float)rotMatArr[1], (float)rotMatArr[2], (float)tvecArr[0]));
//transformationM.SetRow(1, new Vector4((float)rotMatArr[3], (float)rotMatArr[4], (float)rotMatArr[5], (float)tvecArr[1]));
//transformationM.SetRow(2, new Vector4((float)rotMatArr[6], (float)rotMatArr[7], (float)rotMatArr[8], (float)tvecArr[2]));
//transformationM.SetRow(3, new Vector4(0, 0, 0, 1));
//Debug.Log("transformationM " + transformationM.ToString());
success = true;
}
else
{
Debug.Log("not detected");
success = false;
}
return(ARM);
}
private void DetectARUcoMarker()
{
if (downscaleRatio == 1)
{
downScaleRgbaMat = rgbaMat4Thread;
}
else
{
Imgproc.resize(rgbaMat4Thread, downScaleRgbaMat, new Size(), 1.0 / downscaleRatio, 1.0 / downscaleRatio, Imgproc.INTER_LINEAR);
}
Imgproc.cvtColor(downScaleRgbaMat, grayMat, Imgproc.COLOR_RGBA2GRAY);
// Detect markers and estimate Pose
Aruco.detectMarkers(grayMat, dictionary, corners, ids, detectorParams, rejected);
if (applyEstimationPose && ids.total() > 0)
{
Aruco.estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs, tvecs);
for (int i = 0; i < ids.total(); i++)
{
//This example can display ARObject on only first detected marker.
if (i == 0)
{
// Position
double[] tvec = tvecs.get(i, 0);
// Rotation
double[] rv = rvecs.get(i, 0);
Mat rvec = new Mat(3, 1, CvType.CV_64FC1);
rvec.put(0, 0, rv [0]);
rvec.put(1, 0, rv [1]);
rvec.put(2, 0, rv [2]);
Calib3d.Rodrigues(rvec, rotMat);
transformationM.SetRow(0, new Vector4((float)rotMat.get(0, 0) [0], (float)rotMat.get(0, 1) [0], (float)rotMat.get(0, 2) [0], (float)tvec [0]));
transformationM.SetRow(1, new Vector4((float)rotMat.get(1, 0) [0], (float)rotMat.get(1, 1) [0], (float)rotMat.get(1, 2) [0], (float)tvec [1]));
transformationM.SetRow(2, new Vector4((float)rotMat.get(2, 0) [0], (float)rotMat.get(2, 1) [0], (float)rotMat.get(2, 2) [0], (float)(tvec [2] / downscaleRatio)));
transformationM.SetRow(3, new Vector4(0, 0, 0, 1));
// Right-handed coordinates system (OpenCV) to left-handed one (Unity)
ARM = invertYM * transformationM;
// Apply Z axis inverted matrix.
ARM = ARM * invertZM;
hasUpdatedARTransformMatrix = true;
break;
}
}
}
}
public override void Draw(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image)
{
foreach (var arucoGridBoard in arucoTracker.GetArucoObjects <ArucoGridBoard>(dictionary))
{
if (arucoTracker.DrawAxes && cameraParameters != null && arucoGridBoard.Rvec != null)
{
Aruco.DrawAxis(image, cameraParameters.CameraMatrices[cameraId], cameraParameters.DistCoeffs[cameraId],
arucoGridBoard.Rvec, arucoGridBoard.Tvec, arucoGridBoard.AxisLength);
}
}
}
void ArucoDetection()
{
Dictionary dict = Aruco.getPredefinedDictionary(Aruco.DICT_4X4_1000);
Aruco.detectMarkers(cached_initMat, dict, corners, ids);
Aruco.drawDetectedMarkers(cached_initMat, corners, ids);
Debug.Log("AD: Markers Detected");
src_recent_array = new Point[7];
for (int i = 0; i < corners.Count; i++)
{
int aruco_id = (int)(ids.get(i, 0)[0]);
int src_i = arucoTosrc(aruco_id);
int corner_i = aruco_id % 4;
// Store corner[i] into spa[src_i]
src_point_array[src_i] = new Point(corners[i].get(0, corner_i)[0], corners[i].get(0, corner_i)[1]);
src_recent_array[src_i] = new Point(corners[i].get(0, corner_i)[0], corners[i].get(0, corner_i)[1]);
Debug.LogFormat("aruco_id: {0}; corner: {1}; src_i: {2}", aruco_id, src_point_array[src_i], src_i);
// Display the corner as circle on outMat.
Imgproc.circle(cached_initMat, src_point_array[src_i], 10, new Scalar(255, 255, 0));
}
Debug.Log("AD: src_point_array and recent populated");
// Count non-null source points
int markerCount = count_src_nulls();
Debug.LogFormat("AD: markerCount = {0}", markerCount);
m_ImageInfo.text = string.Format("Number of markers detected: {0}", markerCount);
spa_full = (markerCount == 7);
// Check if have valid faces
for (int i = 0; i < 3; i++)
{
// faceX_full[i] = check_faces(i);
faceX_full[i] = check_faces(i);
}
Debug.LogFormat("AD: full faces: 1-{0}, 2-{1}, 3-{2}",
faceX_full[0], faceX_full[1], faceX_full[2]);
for (int i = 0; i < 3; i++)
{
// faceX_full[i] = check_faces(i);
faceX_recent_full[i] = check_recent_faces(i);
}
Debug.LogFormat("AD: recent full faces: 1-{0}, 2-{1}, 3-{2}",
faceX_recent_full[0], faceX_recent_full[1], faceX_recent_full[2]);
Core.flip(cached_initMat, outMat, 0);
Debug.Log("AD: done");
}
void Create(int markerId)
{
ValidMarkerImage();
Dictionary dictionary = Aruco.getPredefinedDictionary(dictionaryId);
Aruco.drawMarker(dictionary, markerId, markerSize, markerImage);
Utils.matToTexture2D(markerImage, texture, true, 0, true);
imageQrCode.texture = texture;
}
public override void Detect(int cameraId, Aruco.Dictionary dictionary, Cv.Mat image)
{
Std.VectorVectorPoint2f markerCorners, rejectedCandidateCorners;
Std.VectorInt markerIds;
Aruco.DetectMarkers(image, dictionary, out markerCorners, out markerIds, arucoTracker.DetectorParameters, out rejectedCandidateCorners);
DetectedMarkers[cameraId][dictionary] = (int)markerIds.Size();
MarkerCorners[cameraId][dictionary] = markerCorners;
MarkerIds[cameraId][dictionary] = markerIds;
RejectedCandidateCorners[cameraId][dictionary] = rejectedCandidateCorners;
}
/// <summary>
/// Calls <see cref="OnPropertyUpdated()"/> in editor mode.
/// </summary>
protected virtual void OnValidate()
{
#if UNITY_EDITOR
if (!UnityEditor.EditorApplication.isPlayingOrWillChangePlaymode)
{
if (Dictionary == null || dictionaryName != Dictionary.Name)
{
dictionary = Aruco.GetPredefinedDictionary(dictionaryName);
}
OnPropertyUpdated();
}
#endif
}
// Update is called once per frame
void Update()
{
UDetectMarkersData _markerData = Aruco.UDetectMarkers(_webCamTexture.GetPixels32(), _webCamTexture.width, _webCamTexture.height);
if (_markerData.markerIds.Length > 0)
{
int[] markersIds = _markerData.markerIds;
List <List <Vector2> > markers = _markerData.markers;
List <List <Vector2> > rejectedCandidates = _markerData.rejectedCandidates;
DrawQuad(markersIds, markers, rejectedCandidates);
}
}
public override void EstimateTransforms(int cameraId, Aruco.Dictionary dictionary)
{
Std.VectorVec3d rvecs = null, tvecs = null;
if (DetectedMarkers[cameraId][dictionary] > 0 && cameraParameters != null)
{
Aruco.EstimatePoseSingleMarkers(MarkerCorners[cameraId][dictionary], EstimatePoseMarkerLength, cameraParameters.CameraMatrices[cameraId],
cameraParameters.DistCoeffs[cameraId], out rvecs, out tvecs);
}
MarkerRvecs[cameraId][dictionary] = rvecs;
MarkerTvecs[cameraId][dictionary] = tvecs;
}
