409 lines
17 KiB
C#
409 lines
17 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora;
|
|
using Aurora.Api;
|
|
using Aurora.Api.DataType;
|
|
using VisionCommon;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Reactive.Linq;
|
|
|
|
namespace VisionCalibrator {
|
|
/// <summary>
|
|
/// 이미지 좌표와 자재 좌표간의 변환을 위한 클래스이다.
|
|
/// </summary>
|
|
public class Calibrator : IDisposable {
|
|
private IEventObservable _EventObservable;
|
|
private Logger _Logger;
|
|
private FovInfo _FovInfo;
|
|
private SubSysInfo _SubSysInfo;
|
|
private CalRecipe _CalRecipe;
|
|
private FovRecipe _FovRecipe;
|
|
private SetupDirection CamSetupDirection = new SetupDirection();
|
|
private FlipAndRotation FlipAndRotation = new FlipAndRotation();
|
|
|
|
/// <summary>
|
|
/// Calibrator 생성자
|
|
/// </summary>
|
|
/// <param name="fovInfo">Fov 정보</param>
|
|
public Calibrator(FovInfo fovInfo) {
|
|
_EventObservable = VisionEnv.AuroraEnv.EventObservable;
|
|
_FovInfo = fovInfo;
|
|
_SubSysInfo = fovInfo.ToSubSysInfo();
|
|
_Logger = new Logger(_FovInfo.SubSysName + _FovInfo.FovName);
|
|
UpdateRecipe();
|
|
UpdateCameraConfig();
|
|
SetObservable();
|
|
}
|
|
|
|
private void SetObservable() {
|
|
var observable = VisionEventId.GetRecipeUpdateObservable();
|
|
observable.Subscribe(_ => UpdateRecipe());
|
|
|
|
Observable.Merge(
|
|
VisionEnv.AuroraEnv.EventObservable.AsObservable(AuroraSysEvent.SYS_DEVICE_ACTIVATED))
|
|
.Where(x => _FovRecipe != null)
|
|
.Where(x => x.Param as string == _FovRecipe.CamRecipe.CamId.ToString())
|
|
.Subscribe(_ => UpdateCameraConfig());
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이미지 좌표를 자재 좌표계로 변환한다
|
|
/// </summary>
|
|
/// <param name="imagePoint">변환할 이미지 좌표</param>
|
|
/// <param name="imageSize">변환할 이미지의 크기</param>
|
|
/// <returns>자재 좌표</returns>
|
|
public StaticPoint TransformToCalibratedSpace(ImagePoint imagePoint, ImageSize imageSize) {
|
|
if (_FovRecipe == null) {
|
|
return new StaticPoint();
|
|
}
|
|
var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
|
|
return TransformToCalibratedSpace(fovCenter, imagePoint, imageSize.Width, imageSize.Height);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이미지 좌표를 자재 좌표계로 변환한다
|
|
/// </summary>
|
|
/// <param name="imagePoint">변환할 이미지 좌표</param>
|
|
/// <param name="imageSize">변환할 이미지의 크기</param>
|
|
/// <param name="errorRotation">변환할 이미지의 회전 오차</param>
|
|
/// <param name="pixelSize">카메라의 픽셀 크기</param>
|
|
/// <returns>자재 좌표</returns>
|
|
public StaticPoint TransformToCalibratedSpace(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, StaticPoint pixelSize) {
|
|
var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
|
|
return TransformToCalibratedSpace(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSize);
|
|
}
|
|
|
|
/// <summary>
|
|
/// X축 픽셀 사이즈만으로 이미지 좌표를 자재 좌표계로 변환한다
|
|
/// </summary>
|
|
/// <param name="imagePoint">변환할 이미지 좌표</param>
|
|
/// <param name="imageSize">변환할 이미지의 크기</param>
|
|
/// <param name="errorRotation">변환할 이미지의 회전 오차</param>
|
|
/// <param name="pixelSizeX">카메라의 X축 픽셀 크기</param>
|
|
/// <returns>자재 좌표</returns>
|
|
public StaticPoint TransformToCalibratedSpaceWithPixelX(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, double pixelSizeX) {
|
|
var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
|
|
return TransformToCalibratedSpaceWithPixelX(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSizeX);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Y축 픽셀 사이즈만으로 이미지 좌표를 자재 좌표계로 변환한다
|
|
/// </summary>
|
|
/// <param name="imagePoint">변환할 이미지 좌표</param>
|
|
/// <param name="imageSize">변환할 이미지의 크기</param>
|
|
/// <param name="errorRotation">변환할 이미지의 회전 오차</param>
|
|
/// <param name="pixelSizeY">카메라의 Y축 픽셀 크기</param>
|
|
/// <returns>자재 좌표</returns>
|
|
public StaticPoint TransformToCalibratedSpaceWithPixelY(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, double pixelSizeY) {
|
|
var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
|
|
return TransformToCalibratedSpaceWithPixelY(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSizeY);
|
|
}
|
|
|
|
internal StaticPoint TransformToCalibratedSpaceWithPixelX(
|
|
StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, double pixelSizeX) {
|
|
// Pixel size [px -> mm]
|
|
var recipePixelSize = _FovRecipe.CalFovRecipe.PixelSize;
|
|
|
|
StaticPoint pixelSize = new StaticPoint(pixelSizeX, recipePixelSize.Y);
|
|
|
|
return TransformToCalibratedSpace(
|
|
fovCenter,
|
|
imagePoint,
|
|
imageWidth,
|
|
imageHeight,
|
|
errorRotation,
|
|
pixelSize);
|
|
}
|
|
|
|
internal StaticPoint TransformToCalibratedSpaceWithPixelY(
|
|
StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, double pixelSizeY) {
|
|
// Pixel size [px -> mm]
|
|
var recipePixelSize = _FovRecipe.CalFovRecipe.PixelSize;
|
|
|
|
StaticPoint pixelSize = new StaticPoint(recipePixelSize.X, pixelSizeY);
|
|
|
|
return TransformToCalibratedSpace(
|
|
fovCenter,
|
|
imagePoint,
|
|
imageWidth,
|
|
imageHeight,
|
|
errorRotation,
|
|
pixelSize);
|
|
}
|
|
|
|
internal StaticPoint TransformToCalibratedSpace(
|
|
StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight) {
|
|
// 카메라 설치 오차 보정 값
|
|
var errorRotation = _FovRecipe.CalFovRecipe.ErrRotationX.Clone() as Angle;
|
|
|
|
// Pixel size [px -> mm]
|
|
var pixelSize = _FovRecipe.CalFovRecipe.PixelSize;
|
|
|
|
return TransformToCalibratedSpace(
|
|
fovCenter,
|
|
imagePoint,
|
|
imageWidth,
|
|
imageHeight,
|
|
errorRotation,
|
|
pixelSize);
|
|
}
|
|
|
|
internal StaticPoint TransformToCalibratedSpace(
|
|
StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, StaticPoint pixelSize) {
|
|
try {
|
|
// FlipY 변환 값
|
|
eFlipY flipY = GetCorrectionFlipY();
|
|
|
|
// RotationX 변환 값
|
|
var setupDirection = GetCorrectionRotationX();
|
|
|
|
var fovCenterOffset = _FovRecipe.CalFovRecipe.FovCenterOffset;
|
|
|
|
var res = TransformImageToStatic(
|
|
fovCenter,
|
|
fovCenterOffset,
|
|
imagePoint,
|
|
setupDirection, // rotCorrectionX, rotErrorX
|
|
errorRotation,
|
|
imageWidth,
|
|
imageHeight,
|
|
(int)flipY,
|
|
pixelSize.X,
|
|
pixelSize.Y);
|
|
return res;
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private static StaticPoint TransformImageToStatic(
|
|
StaticPoint fovCenter,
|
|
StaticPoint fovCenterOffset,
|
|
ImagePoint imagePoint,
|
|
Angle setupDirection, Angle errorRotation,
|
|
int imageWidth, int imageHeight,
|
|
int flipY, double pixelSizeX, double pixelSizeY) {
|
|
try {
|
|
var FovCenter = new PointData() {
|
|
X = fovCenter.X,
|
|
Y = fovCenter.Y
|
|
};
|
|
|
|
var FovCenterOffset = new PointData() {
|
|
X = fovCenterOffset.X,
|
|
Y = fovCenterOffset.Y
|
|
};
|
|
|
|
var ImagePos = new PointData() {
|
|
X = imagePoint.X,
|
|
Y = imagePoint.Y
|
|
};
|
|
|
|
var TargetPos = NativeCalibrator.ImageToStatic(FovCenter, FovCenterOffset,
|
|
setupDirection.Radian, errorRotation.Radian,
|
|
imageWidth, imageHeight,
|
|
flipY, pixelSizeX, pixelSizeY, ImagePos);
|
|
|
|
return TargetPos;
|
|
}
|
|
catch (Exception e) {
|
|
}
|
|
return new StaticPoint();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 좌표를 이미지 좌표계로 변환한다
|
|
/// </summary>
|
|
/// <param name="staticPoint">변환할 자재 좌표</param>
|
|
/// <param name="imageSize">변환할 이미지의 크기</param>
|
|
/// <returns>이미지 좌표</returns>
|
|
public ImagePoint TransformToImagePoint(StaticPoint staticPoint, ImageSize imageSize) {
|
|
return InverseTransformToCalibratedSpace(
|
|
_FovRecipe.CalFovRecipe.FovCenter,
|
|
staticPoint,
|
|
imageSize.Width,
|
|
imageSize.Height,
|
|
_FovRecipe.CalFovRecipe.ErrRotationX,
|
|
_FovRecipe.CalFovRecipe.PixelSize
|
|
);
|
|
}
|
|
|
|
internal ImagePoint InverseTransformToCalibratedSpace(
|
|
StaticPoint fovCenter, StaticPoint staticPoint, int imageWidth, int imageHeight, Angle errorRotation, StaticPoint pixelSize) {
|
|
try {
|
|
// FlipY 변환 값
|
|
eFlipY flipY = GetCorrectionFlipY();
|
|
|
|
// RotationX 변환 값
|
|
var setupDirection = GetCorrectionRotationX();
|
|
|
|
var fovCenterOffset = _FovRecipe.CalFovRecipe.FovCenterOffset;
|
|
|
|
var res = TransformStaticToImage(
|
|
fovCenter,
|
|
fovCenterOffset,
|
|
staticPoint,
|
|
setupDirection, // rotCorrectionX, rotErrorX
|
|
errorRotation,
|
|
imageWidth,
|
|
imageHeight,
|
|
(int)flipY,
|
|
pixelSize.X,
|
|
pixelSize.Y);
|
|
return res;
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private static ImagePoint TransformStaticToImage(
|
|
StaticPoint fovCenter,
|
|
StaticPoint fovCenterOffset,
|
|
StaticPoint staticPoint,
|
|
Angle setupDirection, Angle errorRotation,
|
|
int imageWidth, int imageHeight,
|
|
int flipY, double pixelSizeX, double pixelSizeY) {
|
|
try {
|
|
var FovCenter = new PointData() {
|
|
X = fovCenter.X,
|
|
Y = fovCenter.Y
|
|
};
|
|
|
|
var FovCenterOffset = new PointData() {
|
|
X = fovCenterOffset.X,
|
|
Y = fovCenterOffset.Y
|
|
};
|
|
|
|
var StaticPos = new PointData() {
|
|
X = staticPoint.X,
|
|
Y = staticPoint.Y
|
|
};
|
|
|
|
var TargetPos = NativeCalibrator.StaticToImage(FovCenter, FovCenterOffset,
|
|
setupDirection.Radian, errorRotation.Radian,
|
|
imageWidth, imageHeight,
|
|
flipY, pixelSizeX, pixelSizeY, StaticPos);
|
|
|
|
return TargetPos;
|
|
}
|
|
catch (Exception e) {
|
|
}
|
|
return new ImagePoint();
|
|
}
|
|
|
|
//View Model에서 호출
|
|
/// <summary>
|
|
/// 카메라의 픽셀 크기를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaImagePoints">이미지 좌표 리스트</param>
|
|
/// <param name="moveLength">측정시 이동 거리</param>
|
|
/// <param name="errorRot">회전 오차</param>
|
|
/// <param name="target">계산할 픽셀 크기의 축</param>
|
|
/// <returns>픽셀 크기</returns>
|
|
public double GetPixelSize(List<ImagePoint> meaImagePoints, double moveLength, double errorRot, char target) {
|
|
// target parameter ErrorRot enum 활용
|
|
double pixelSize = 0;
|
|
pixelSize = GetPixelSizeInternal(meaImagePoints, moveLength, errorRot, target);
|
|
|
|
return pixelSize;
|
|
}
|
|
private static double GetPixelSizeInternal(List<ImagePoint> meaImagePoints, double moveLength, double errorRot, char target) {
|
|
return NativeCalibrator.CalibrateImagePixel(meaImagePoints, moveLength, errorRot, target);
|
|
}
|
|
|
|
//View Model에서 호출
|
|
/// <summary>
|
|
/// 회전 오차를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoints">자재 좌표 리스트</param>
|
|
/// <param name="target">계산시 사용할 데이터의 축</param>
|
|
/// <returns>회전 오차</returns>
|
|
public Angle FindStaticErrorRotation(List<ImagePoint> meaPoints, char target) {
|
|
List<ImagePoint> meaPointsY = new List<ImagePoint>();
|
|
foreach (var staticPoint in meaPoints) {
|
|
_Logger.FuncI($"meaPoints Data : {staticPoint}");
|
|
if (target == 'y') {
|
|
meaPointsY.Add(new ImagePoint(staticPoint.Y, staticPoint.X * -1));
|
|
}
|
|
}
|
|
if (target == 'y') {
|
|
return FindStaticErrorRotationInternal(meaPointsY);
|
|
}
|
|
|
|
return FindStaticErrorRotationInternal(meaPoints);
|
|
}
|
|
private static Angle FindStaticErrorRotationInternal(List<ImagePoint> meaPoints) {
|
|
return NativeCalibrator.StaticCalibrateErrorRot(meaPoints);
|
|
}
|
|
|
|
//View Model에서 호출
|
|
/// <summary>
|
|
/// 회전 원점을 계산한다.
|
|
/// </summary>
|
|
/// <param name="measuredData">자재 좌표 리스트</param>
|
|
/// <param name="planRotation">회전한 각도</param>
|
|
/// <returns>회전 원점의 자재 좌표</returns>
|
|
public StaticPoint GetThetaOriginPoint(List<StaticPoint> measuredData, Angle planRotation) {
|
|
_Logger.FuncI($"Plan Rotation : {planRotation}");
|
|
Angle rotation = CoordSystem.DenormalizeAngleDirection(planRotation, (int)_CalRecipe.CoordSystem.FlipY);
|
|
StaticPoint thetaOrigin = GetThetaOriginPointInternal(measuredData, rotation);
|
|
|
|
return thetaOrigin;
|
|
}
|
|
|
|
private static StaticPoint GetThetaOriginPointInternal(List<StaticPoint> measuredData, Angle rotation) {
|
|
StaticPoint thetaOrigin = NativeCalibrator.StaticCalibrateThetaOrigin(measuredData, rotation);
|
|
|
|
return thetaOrigin;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Fov 레시피를 새로 가져온다.
|
|
/// </summary>
|
|
/// <param name="fovRecipe">Fov 레시피</param>
|
|
public void RefreshFovRecipe(FovRecipe fovRecipe) {
|
|
_FovRecipe = fovRecipe;
|
|
}
|
|
|
|
private void UpdateRecipe() {
|
|
_CalRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
|
|
_FovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfo);
|
|
}
|
|
|
|
private void UpdateCameraConfig() {
|
|
var camRecipe = VisionRecipeManager.GetCamRecipe(_FovInfo);
|
|
if (camRecipe != null) {
|
|
var CameraConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(camRecipe.CamId.ToString()) as CamConfig;
|
|
CamSetupDirection = CameraConfig.SetupDirection.Clone() as SetupDirection;
|
|
FlipAndRotation = CameraConfig.FlipAndRotation.Clone() as FlipAndRotation;
|
|
}
|
|
}
|
|
|
|
internal Angle GetCorrectionRotationX() {
|
|
return Angle.CreateWithDegree(
|
|
(_CalRecipe.CoordSystem.RotationX.Degree - CamSetupDirection.RotationX) * (int)CamSetupDirection.FlipY);
|
|
}
|
|
|
|
internal eFlipY GetCorrectionFlipY() {
|
|
return (eFlipY)((int)_CalRecipe.CoordSystem.FlipY * (int)CamSetupDirection.FlipY);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 좌표계에 Y축 반전을 적용 여부를 가져온다.
|
|
/// </summary>
|
|
/// <returns>Y축 반전 여부</returns>
|
|
public eFlipY GetFlipY() {
|
|
return _CalRecipe.CoordSystem.FlipY;
|
|
}
|
|
|
|
public void Dispose() {
|
|
}
|
|
}
|
|
} |