Hand Eye Calibrator 3D

1 개요

Robot End Effector와 Camera 간의 변환 행렬을 추정하는 알고리즘입니다.

Fig. Camera_from_Hand, Robot End Effector(Hand)와 Camera간 변환 관계

위 결과는 Eye_In_Hand 상황의 Camera_from_Hand에 해당합니다.

2 알고리즘 상세 설명

Calibration Object	Hand Eye Calibration Result

Fig. 촬영된 Chess Board 이미지 및 Hand Eye Calibration 결과(Eye-in-hand)

위 사진의 Calibration Object는 Robot End Effector에 부착된 카메라에서 촬영된 이미지이며, 오른쪽 Hand Eye Calibration Result의 End Effector는 Calibration Object가 촬영된 시점의 Robot Base 좌표입니다. Chess Board 이미지에서 카메라 외부 행렬을 계산하기 위해서 Camera Pose 3D 알고리즘을 수행해야 합니다.
오른쪽 사진과 같이 End Effector 에서 Camera, Camera에서 Calibration Board로 변환한 재투영 관계를 확인하였을 때 높은 정확도로 변환 행렬을 추정하는 것을 알 수 있습니다.

3 파라미터 설정 및 사용 방법

Calibration을 위한 Camera 내부 파라미터 설정 방법은 Camera Pose 3D와 동일합니다.

SetScaleFactor(double f64ScaleFactor)
- 설정한 End Effector의 축척 값을 설정합니다.
- 캘리브레이션 보드가 촬영된 이미지가 설정되어 캘리브레이션이 동작하는 경우 축척은 아래 기준을 따릅니다.
  - 알고리즘 내부적으로 설정된 Camera Pose의 기본 단위는 meter 단위입니다.
  - End Effector의 축척이 millimeter일 경우 Scale Factor는 0.001을 설정해야합니다.
- 이미지가 설정되지 않았으며, 계산된 Camera Pose가 직접 입력되는 경우 축척은 Camera Pose를 기준으로 합니다.
  - Camera Pose가 millimeter 단위이며, End Effector가 meter 단위인 경우 Scale Factor는 1000을 설정해야합니다.
  - f64ScaleFactor : 축척
SetResultScaleFactor(double f64ScaleFactor)
- 결과 Translation의 축척을 변경합니다.
- 결과 Translation이 Millimeter일 때 원하는 축척이 meter 단위일 경우 0.001을 설정해야합니다.
- f64ScaleFactor : 축척
SetCalibrationCameraMatrix(CFLPoint<double> flpPrincipalPoint, CFLPoint<double> flpFocalLength)
- 카메라 내부 파라미터를 설정합니다.
- flpFocalLength, flpPrincipalPoint : 카메라의 초점 거리 및 주점
SetCalibrationDistortionCoefficient(CFLArray<double> flaDistortionCoeff)
- 카메라 왜곡 계수를 설정합니다.
- flaDistortionCoeff : 카메라 왜곡 계수
SetCalibrationBoardCellPitch(CFLPoint<double> flpBoardCellPitch)
- 캘리브레이션 보드의 셀 간격을 설정합니다.
- flpBoardCellPitch : 캘리브레이션 보드의 셀 간격

카메라 외부 행렬 설정 방법

Camera 행렬은 촬영된 이미지, 혹은 이미 계산된 카메라 외부 행렬로 설정할 수 있습니다.
- 촬영된 이미지로 설정하는 방법
  - SetSourceImage(CFLImage fliImage)
- 이미 계산된 카메라 외부 행렬로 설정하는 방법
  - SetCalibrationCameraCoordinateTranslation(CFLArray<TPoint3<double>> flaCameraTranslation)
    - 카메라 이동 벡터 배열을 설정합니다.
  - SetCalibrationCameraCoordinateAngle(CFLArray<TPoint3<double>> flaCameraAngle)
    - 카메라 회전 벡터 혹은 각도 배열을 설정합니다.

엔드 이펙터 설정 방법

Robot Base End Effector의 Pose를 설정하기 위해 아래와 같은 함수를 사용할 수 있습니다.
- SetCalibrationEndEffectorCoordinateTranslation(CFLArray<TPoint3<double>> flaEndEffectorTranslation)
  - End Effector의 이동 벡터 배열을 설정합니다.
- SetCalibrationEndEffectorCoordinateAngle(CFLArray<TPoint3<double>> flaEndEffectorAngle)
  - End Effector의 회전 벡터 혹은 각도 배열을 설정합니다.
(.csv, .txt) 포맷으로 저장된 데이터를 불러올 수 있습니다.
- LoadEndEffectorPose(CFLString<wchar_t> flsFileName)
- 아래와 같은 포맷을 준수해야합니다.
Fig. Robot End Effector 포맷(Tx, Ty, Tz, Rx, Ry, Rz)
End Effector의 각도 단위, 회전 타입을 설정해야합니다.
- SetRotationType(ERotationType eType)
  - ERotationType_RotationVector
    - 회전 벡터 타입입니다.
  - ERotationType_EulerAngle
    - 회전 각도 타입입니다.
- SetEndEffectorAngleUnit(ImageProcessing::EAngleUnit eType)
  - EAngleUnit_Degree
  - EAngleUnit_Radian
SetCalibrationMode(ECalibrationMode eMode)
- Calibration Type을 설정합니다.
  - ECalibrationMode_EyeInHand : Eye in hand 상황에 적합합니다. End Effector에 Camera가 부착된 경우 사용합니다.
  - ECalibrationMode_EyeToHand : Eye to hand 상황에 적합합니다. Camera가 고정된 위치에 있는 경우 사용합니다.

4.알고리즘 수행 결과 취득 방법

GetResultHandToEyeRotationVector(CFLArray<double> flaResultRotationVector)
- 결과 회전 벡터를 얻어옵니다.
- End Effector -> Camera 결과를 반환합니다.
GetResultHandToEyeTranslationVector(CFLArray<double> flaResultTranslationVector)
- 결과 이동 벡터를 얻어옵니다.
- End Effector -> Camera 결과를 반환합니다.
GetResultHandToEyeRotationMatrix(CMatrix<double> matResultRotationMatrix)
- 결과 회전 행렬을 얻어옵니다.
- End Effector -> Camera 결과를 반환합니다.
GetResultHandToEyeEulerAngle(CFLArray<double> flaResultEulerAngle)
- 결과 회전 각도를 얻어옵니다.
- End Effector -> Camera 결과를 반환합니다.
GetResultHandToEyeTransformationMatrix(CMatrix<double> matResultTransformationMatrix)
- 결과 변환 행렬을 얻어옵니다.
- End Effector -> Camera 결과를 반환합니다.
GetResultEyeToHandRotationVector(CFLArray<double> flaResultRotationVector)
- 결과 회전 벡터를 얻어옵니다.
- Camera -> End Effector 결과를 반환합니다.
GetResultEyeToHandTranslationVector(CFLArray<double> flaResultTranslationVector)
- 결과 이동 벡터를 얻어옵니다.
- Camera -> End Effector 결과를 반환합니다.
GetResultEyeToHandRotationMatrix(CMatrix<double> matResultRotationMatrix)
- 결과 회전 행렬을 얻어옵니다.
- Camera -> End Effector 결과를 반환합니다.
GetResultEyeToHandEulerAngle(CFLArray<double> flaResultEulerAngle)
- 결과 회전 각도를 얻어옵니다.
- Camera -> End Effector 결과를 반환합니다.
GetResultEyeToHandTransformationMatrix(CMatrix<double> matResultTransformationMatrix)
- 결과 변환 행렬을 얻어옵니다.
- Camera -> End Effector 결과를 반환합니다.

5 좌표계 변환 방법

Eye In Hand
- Robot Base 좌표계 상의 End Effector 6D Pose 와 Camera 좌표계 상의 6D Pose를 설정해야합니다.
- 변환 결과는 아래와 같습니다.
  - T_Base2Object = T_Base2EE * T_EE2Cam * T_Cam2Object
    - T_EE2Cam : 알고리즘 수행 결과
    - T_Base2EE : End Effector 6D Pose
    - T_Cam2Object : Camera 좌표계 상의 6D Pose
- End Effector 6D Pose는 아래 API로 설정할 수 있습니다.
  - SetConvertingEndEffectorPoseMatrix()
  - 첫 번째 인자 : Translation Vector
  - 두 번째 인자 : Rotation
    - Rotation 설정은 여러 방법으로 할 수 있습니다. 오일러 각도 혹은 회전 벡터 타입은 End Effector의 회전 타입에 종속됩니다.
      1. Euler Angle : TPoint3<double>
      2. Rotation Vector : TPoint3<double>
      3. 임의의 평면 : CFLQuad3<double>
      4. 입력된 정점 데이터에서 추정 : CFLArray<TPoint3<float>>
- Camera 좌표계 상의 6D Pose는 아래 API로 설정할 수 있습니다.
  - SetConvertingCameraPoseMatrix()
  - 첫 번째 인자 : Translation Vector
  - 두 번째 인자 : Rotation
    - Rotation 설정은 여러 방법으로 할 수 있습니다. 오일러 각도 혹은 회전 벡터 타입은 End Effector의 회전 타입에 종속됩니다.
      1. Euler Angle : TPoint3<double>
      2. Rotation Vector : TPoint3<double>
      3. 임의의 평면 : CFLQuad3<double>
      4. 입력된 정점 데이터에서 추정 : CFLArray<TPoint3<float>>
Eye To Hand
- Camera 좌표계 상의 6D Pose를 설정해야합니다.
- 변환 결과는 아래와 같습니다.
  - T_Base2Object = T_Base2Cam * T_Cam2Object
    - T_Base2Cam : 알고리즘 수행 결과
    - T_Cam2Object : Camera 좌표계 상의 6D Pose
  - Camera 좌표계 상의 6D Pose 설정 방법은 Eye In Hand와 동일합니다.
설정 후 Execute() 실행 시 Robot Base 좌표계로 변환된 결과를 얻을 수 있으며, 변환 결과는 아래 API를 사용해 얻을 수 있습니다.
- GetConvertingResultPoseMatrix(TPoint3<double>& tp3Translation, TPoint3<double>& tp3Angle)
  - tp3Angle : End Effector의 회전 타입에 종속된 결과입니다.
- GetConvertingResultPoseMatrix(CMatrix<double>& matResult)

6 예제 코드

CHandEyeCalibrator3D HandEyeCalibrator3D;
CFLImage fliSourceImage;

fliSourceImage.Load(L"C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/ChessBoard.flif");
HandEyeCalibrator3D.SetSourceImage(fliSourceImage);
HandEyeCalibrator3D.LoadEndEffectorPose(L"C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/EndEffectorPose.csv"); // End Effector 포즈 불러오기
HandEyeCalibrator3D.SetCalibrationObjectType(ThreeDim::ECalibrationObjectType::ECalibrationObjectType_ChessBoard); // 캘리브레이션 객체 타입을 체스 보드로 설정
HandEyeCalibrator3D.SetRotationType(ThreeDim::CHandEyeCalibrator3D::ERotationType_RotationVector); // End Effector 회전 타입 설정
HandEyeCalibrator3D.SetOptimizationMethod(ThreeDim::CHandEyeCalibrator3D::EOptimizationMethod::EOptimizationMethod_Nonlinear); // 최적화 방법 설정
HandEyeCalibrator3D.SetEndEffectorAngleUnit(ImageProcessing::EAngleUnit::EAngleUnit_Radian); // End Effector 회전 각도 단위 설정

// 오일러 각 회전 순서
// SetRotationType(CHandEyeCalibrator3D::ERotationType_EulerAngle)시 설정
HandEyeCalibrator3D.SetEulerSequence(Foundation::EEulerSequence::EEulerSequence_Extrinsic_XYZ); 

HandEyeCalibrator3D.SetScaleFactor(1.000000); // End Effector Scale Factor 설정
HandEyeCalibrator3D.SetResultScaleFactor(1.000000); // 결과 Scale Factor 설정

HandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.000001); // 최적화 정확도 설정

CFLArray<double> flaDistortionCoef;

flaDistortionCoef.PushBack(-0.053853);
flaDistortionCoef.PushBack(0.059036);
flaDistortionCoef.PushBack(0.000375);
flaDistortionCoef.PushBack(0.000786);
flaDistortionCoef.PushBack(-0.018948);
HandEyeCalibrator3D.SetCalibrationDistortionCoefficient(flaDistortionCoef); 왜곡 계수 설정

CFLPoint<double> flpFocalLength;
CFLPoint<double> flpPrincipalPoint;

flpFocalLength.x = 428.668823; 
flpFocalLength.y = 428.268188;
flpPrincipalPoint.x = 422.934998; 
flpPrincipalPoint.y = 240.188660;	
HandEyeCalibrator3D.SetCalibrationCameraMatrix(flpFocalLength, flpPrincipalPoint); // 카메라 내부 행렬 설정
HandEyeCalibrator3D.SetCalibrationBoardCellPitch(15.000000, 15.000000); // 체스 보드 셀 간격 설정
HandEyeCalibrator3D.Calibrate();

CHandEyeCalibrator3D HandEyeCalibrator3D = new CHandEyeCalibrator3D();
CFLImage fliSourceImage = new CFLImage();
fliSourceImage.Load("C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/ChessBoard.flif");
HandEyeCalibrator3D.SetSourceImage(ref fliSourceImage);
HandEyeCalibrator3D.LoadEndEffectorPose("C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/EndEffectorPose.csv"); // End Effector 포즈 불러오기
HandEyeCalibrator3D.SetCalibrationObjectType(ECalibrationObjectType.ChessBoard); // 캘리브레이션 객체 타입을 체스 보드로 설정
HandEyeCalibrator3D.SetRotationType(ERotationType.RotationVector); // End Effector 회전 타입 설정
HandEyeCalibrator3D.SetOptimizationMethod(EOptimizationMethod.Nonlinear); // 최적화 방법 설정
HandEyeCalibrator3D.SetEndEffectorAngleUnit(EAngleUnit.Radian); // End Effector 회전 각도 단위 설정
HandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.000010); // 최적화 정확도 설정

// 오일러 각 회전 순서
// SetRotationType(CHandEyeCalibrator3D::ERotationType_EulerAngle)시 설정
HandEyeCalibrator3D.SetEulerSequence(EEulerSequence.Extrinsic_XYZ);

HandEyeCalibrator3D.SetScaleFactor(1.000000); // End Effector Scale Factor 설정
HandEyeCalibrator3D.SetResultScaleFactor(1.000000); // 결과 Scale Factor 설정

List<double> listDistortionCoef = new List<double>();
CFLPoint<double> flpFocalLength = new CFLPoint<double>();
CFLPoint<double> flpPrincipalPoint = new CFLPoint<double>();

listDistortionCoef.Add(-0.053853);
listDistortionCoef.Add(0.059036);
listDistortionCoef.Add(0.000375);
listDistortionCoef.Add(0.000786);
listDistortionCoef.Add(-0.018948);
flpFocalLength.x = 428.668823; 
flpFocalLength.y = 428.268188;
flpPrincipalPoint.x = 422.934998; 
flpPrincipalPoint.y = 240.188660;

HandEyeCalibrator3D.SetCalibrationDistortionCoefficient(listDistortionCoef); // 왜곡 계수 설정
HandEyeCalibrator3D.SetCalibrationCameraMatrix(flpFocalLength, flpPrincipalPoint); // 카메라 내부 행렬 설정
HandEyeCalibrator3D.SetCalibrationBoardCellPitch(15.000000, 15.000000);
HandEyeCalibrator3D.Calibrate();

HandEyeCalibrator3D = CHandEyeCalibrator3D()
fliSourceImage = CFLImage()
fliSourceImage.Load("C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/ChessBoard.flif")
HandEyeCalibrator3D.SetSourceImage(fliSourceImage)
HandEyeCalibrator3D.LoadEndEffectorPose("C:/Users/Public/Documents/FLImaging/ExampleImages/HandEyeCalibrator3D/EndEffectorPose.csv") # End Effector 포즈 불러오기
HandEyeCalibrator3D.SetCalibrationObjectType(ECalibrationObjectType.ChessBoard) # 캘리브레이션 객체 타입을 체스 보드로 설정
HandEyeCalibrator3D.SetRotationType(ERotationType.RotationVector) # End Effector 회전 타입 설정
HandEyeCalibrator3D.SetOptimizationMethod(EOptimizationMethod.Nonlinear) # 최적화 방법 설정
HandEyeCalibrator3D.SetEndEffectorAngleUnit(EAngleUnit.Radian) # End Effector 회전 각도 단위 설정
HandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.000010) # 최적화 정확도 설정

# 오일러 각 회전 순서
# SetRotationType(CHandEyeCalibrator3D.ERotationType.EulerAngle)시 설정
HandEyeCalibrator3D.SetEulerSequence(EEulerSequence.Extrinsic_XYZ)

HandEyeCalibrator3D.SetScaleFactor(1.000000) # End Effector Scale Factor 설정
HandEyeCalibrator3D.SetResultScaleFactor(1.000000) # 결과 Scale Factor 설정

listDistortionCoef = List[Double]()
flpFocalLength = CFLPoint[Double]()
flpPrincipalPoint = CFLPoint[Double]()

listDistortionCoef.Add(-0.053853)
listDistortionCoef.Add(0.059036)
listDistortionCoef.Add(0.000375)
listDistortionCoef.Add(0.000786)
listDistortionCoef.Add(-0.018948)
flpFocalLength.x = 428.668823
flpFocalLength.y = 428.268188
flpPrincipalPoint.x = 422.934998
flpPrincipalPoint.y = 240.188660

HandEyeCalibrator3D.SetCalibrationDistortionCoefficient(listDistortionCoef) # 왜곡 계수 설정
HandEyeCalibrator3D.SetCalibrationCameraMatrix(flpFocalLength, flpPrincipalPoint) # 카메라 내부 행렬 설정
HandEyeCalibrator3D.SetCalibrationBoardCellPitch(15.000000, 15.000000)
HandEyeCalibrator3D.Calibrate()