Point Cloud Based Hand Eye Calibrator 3D

1 개요

Robot End Effector와 Camera 간의 변환 행렬을 추정하는 알고리즘입니다. 결과 사용 방법 및 파라미터 설명은 Hand Eye Calibrator 3D와 동일합니다.

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

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

2 알고리즘 상세 설명

Hand Eye Calibrator 3D와 다르게 추정 시 필요한 T_Cam2Object를 3D Match를 통해 추정합니다.

T_Cam2Object 추정 방법
- Hand Eye Calibrator 3D
  - T_Cam2Object : 체스보드와 같은 Calibration Object 2D 촬영 결과에서 Camera Pose 3D 알고리즘을 사용해 추정합니다.
- Point Cloud Based Hand Eye Calibrator 3D
  - T_Cam2Object : 3D 촬영 및 3D Match를 동작할 수 있는 모든 데이터를 사용할 수 있습니다.
    - 3D Match 결과는 아래 알고리즘을 동작하여 얻을 수 있습니다.
      - Surface Match 3D
      - Vertex Match 3D

Calibration Object(2D)	Calibration Object(3D)	3D Template	3D Match Result

Fig. Chess Board 이미지 및 3D 매치 결과

알고리즘은 아래 두 상황에 사용 할 수 있습니다.
- Eye In Hand : 카메라가 로봇에 부착.
  - Calibration Object가 지면 등에 고정 된 환경에서 3D 데이터-로봇 포즈를 취득합니다.
- Eye To Hand : 카메라가 로봇 외부에 고정.
  - Calibration Object가 로봇에 부착된 환경에서 3D 데이터-로봇 포즈를 취득합니다.
- n번의 카메라 취득 -> 로봇 포즈 기록 데이터를 사용하여 캘리브레이션 합니다.
  - 10개 이상의 데이터 쌍을 권장하며, 각 로봇 포즈는 다양한 각도(회전 축 기준 최소 15도 이상이 유의미한 차이) 및 이동(Calibration Object가 FOV를 넘지 않는 것을 기준으로 함)이 있어야 높은 정확도의 결과를 얻을 수 있습니다.

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

SetOptimizationMethod(EOptimizationMethod eMethod)
- EOptimizationMethod_Linear : 선형 방법으로 AX=XB에 대한 Closed Form을 계산합니다.
- EOptimizationMethod_Nonlinear : 비선형 방법으로 캘리브레이션하며, AX=XB 오차를 최소화 합니다.
- EOptimizationMethod_Linear_ReprojectionBased : 선형 방법으로 캘리브레이션 된 결과를 초기 행렬로 사용하여 AX=ZB 오차를 최소화 합니다.
- EOptimizationMethod_Nonlinear_ReprojectionBased : 비선형 방법으로 캘리브레이션 된 AX=XB결과를 초기 행렬로 사용하여 AX=ZB 오차를 최소화 합니다.
SetScaleFactor(double f64ScaleFactor)
- 설정한 End Effector의 축척 값을 설정합니다.
  - Source Object의 축척 값에 맞게 설정해야 합니다.
- f64ScaleFactor : 축척
SetResultScaleFactor(double f64ScaleFactor)
- 결과 Translation의 축척을 변경합니다.
- 결과 Translation이 Millimeter일 때 원하는 축척이 meter 단위일 경우 0.001을 설정해야합니다.
- f64ScaleFactor : 축척

3D Match 설정 및 Source Object 설정 방법

3D Match 동작에 필요한 매치 파일을 설정해야 합니다.
- 아래 세 가지 방법으로 설정할 수 있습니다.
  - Set3DMatchModel(CSurfaceMatch3D)
  - Set3DMatchModel(CVertexMatch3D)
    - 학습이 완료된 Surface/Vertex Match 3D 객체를 설정합니다.
      - 2 알고리즘 상세 설명의 3D Template과 같이 매칭할 객체를 학습해야 합니다.
        
        학습 및 매치 실행 방법은 아래 문서를 참고 바랍니다.
        
        Surface Match 3D
        
        Vertex Match 3D
  - Set3DMatchModel(CFLStringW)
    - Surface/Vertex Match 3D의 저장 파일을 Load합니다.
Source Object
- 기록한 End Effector 좌표와 해당 좌표에서 촬영된 3D 데이터를 의미합니다.
- 반드시 3D Template으로 설정한 데이터가 포함되어 있어야 합니다.
- AddSourceObject(CFL3DObject)
  - 촬영한 데이터 개수 만큼 호출해야 합니다.
  - Source Object 및 End Effector 좌표는 위 사진의 Load End Effector 혹은 Add Trajectory 버튼을 사용해 설정할 수 있습니다.
  - Load End Effector
    - *.txt, *.csv로 기록된 파일을 로드합니다.
      - 기록된 행 만큼 Trajectory가 생성되며, 각 Trajectory에서 Source File Path 를 설정할 수 있습니다.
  - Add Trajectory
    - Trajectory를 추가하여 설정할 수 있습니다.
  - Add Object
    - Check 해제 시 Calibrate 동작 후 결과 정확도를 저해하는 Trajectory를 제외할 수 있습니다.
    - 노이즈가 심하거나 데이터 영역이 매치 학습 데이터에 비해 너무 넓은 경우 다음과 같은 방법을 고려할 수 있습니다.
      - 3D Match 클래스에 ROI를 지정.
      - 해당 Trajectory의 Source File 데이터를 Load한 뒤 해당 데이터 일부만 Crop 후 사용.
  - 3D Match Test
    - 매치 결과를 지정한 Match Test 3D View에서 확인할 수 있습니다.

엔드 이펙터 설정 방법

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)
캘리브레이션 결과를 사용하였을 때 타겟 지점에 대해 오차가 발생한다면 다음과 같이 오프셋 설정을 할 수 있습니다.
- SetConvertingPoseOffsetMatrix(CMatrix<double>& matPoseOffset)
  - 4x4 Rigid Transformation Matrix를 입력으로 오프셋 설정.
- SetConvertingPoseOffsetMatrix(TPoint3<double>& tp3Translation, TPoint3<double>& tp3Angle)
  - tp3Translation : 캘리브레이션 결과와 동일한 축척을 가져야합니다.
  - tp3Angle : 캘리브레이션 시 설정한 Rotation Type, Angle Unit, Euler Sequence와 동일하게 설정해야합니다.
- SetCoordinateConvertingMethod(ECoordinateConvertingMethod eMethod)
  - ECoordinateConvertingMethod_WithCameraOffset : Camera 기준의 Offset을 설정합니다. Local 좌표계(Tool, TCP)의 축 방향대로 Translation Offset이 적용됩니다.
  - ECoordinateConvertingMethod_WithBaseOffset : Base 기준의 Offset을 설정합니다. World 좌표계(Robot Base)의 축 방향대로 Translation Offset이 적용됩니다.

6 예제 코드

CPointCloudBasedHandEyeCalibrator3D PointCloudBasedHandEyeCalibrator3D;

// 3D 매치 모델 설정
// Surface Match 3D 혹은 Vertex Match 3D의 모델 파일 경로 설정
// 확장자 : *.flsm, *.flvm
PointCloudBasedHandEyeCalibrator3D.Set3DMatchModel(L"");

//////////////////////////////////////////////
// CSV 파일로 포즈를 불러올 경우
PointCloudBasedHandEyeCalibrator3D.LoadEndEffectorPose("[CSV File Path].csv");
//////////////////////////////////////////////
// 직접 추가할 경우
CFLArray<TPoint3<double>> flaEndEffectorCoordinateTranslations;
flaEndEffectorCoordinateTranslations.PushBack(TPoint3<double>(0, 0, 0));
//		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateTranslation(flaEndEffectorCoordinateTranslations);
CFLArray<TPoint3<double>> flaEndEffectorCoordinateAngles;
flaEndEffectorCoordinateAngles.PushBack(TPoint3<double>(0, 0, 0));
//		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateAngle(flaEndEffectorCoordinateAngles);
//////////////////////////////////////////////

PointCloudBasedHandEyeCalibrator3D.SetRotationType(ThreeDim::ERotationType::ERotationType_EulerAngle); // End Effector 회전 타입 설정
PointCloudBasedHandEyeCalibrator3D.SetOptimizationMethod(ThreeDim::EOptimizationMethod::EOptimizationMethod_Nonlinear); // 최적화 방법 설정
PointCloudBasedHandEyeCalibrator3D.SetEndEffectorAngleUnit(ImageProcessing::EAngleUnit::EAngleUnit_Degree); // End Effector 회전 각도 단위 설정
PointCloudBasedHandEyeCalibrator3D.SetCalibrationMode(ThreeDim::CHandEyeCalibrator3D::ECalibrationMode_EyeInHand); // 캘리브레이션 모드 설정

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

PointCloudBasedHandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.0000001); // 최적화 정확도 설정
PointCloudBasedHandEyeCalibrator3D.SetScaleFactor(1.000000); // End Effector Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.SetResultScaleFactor(1.000000); // 결과 Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.Calibrate();

CPointCloudBasedHandEyeCalibrator3D PointCloudBasedHandEyeCalibrator3D = new CPointCloudBasedHandEyeCalibrator3D();

// 3D 매치 모델 설정
// Surface Match 3D 혹은 Vertex Match 3D의 모델 파일 경로 설정
// 확장자 : *.flsm, *.flvm
PointCloudBasedHandEyeCalibrator3D.Set3DMatchModel("");

//////////////////////////////////////////////
// CSV 파일로 포즈를 불러올 경우
PointCloudBasedHandEyeCalibrator3D.LoadEndEffectorPose("CSV File Path");
//////////////////////////////////////////////
// 직접 추가할 경우
List<TPoint3<double>> listEndEffectorCoordinateTranslations = new List<TPoint3<double>>();
listEndEffectorCoordinateTranslations.Add(new TPoint3<double>(0, 0, 0));
//		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateTranslation(listEndEffectorCoordinateTranslations);
List<TPoint3<double>> listEndEffectorCoordinateAngles = new List<TPoint3<double>>();
listEndEffectorCoordinateAngles.Add(new TPoint3<double>(0, 0, 0));
//		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateAngle(listEndEffectorCoordinateAngles);
//////////////////////////////////////////////
 
PointCloudBasedHandEyeCalibrator3D.SetRotationType(ERotationType.EulerAngle); // End Effector 회전 타입 설정
PointCloudBasedHandEyeCalibrator3D.SetOptimizationMethod(EOptimizationMethod.Nonlinear); // 최적화 방법 설정
PointCloudBasedHandEyeCalibrator3D.SetEndEffectorAngleUnit(EAngleUnit.Degree); // End Effector 회전 각도 단위 설정
PointCloudBasedHandEyeCalibrator3D.SetCalibrationMode(CHandEyeCalibrator3D.ECalibrationMode.EyeInHand); // 캘리브레이션 모드 설정

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

PointCloudBasedHandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.0000001); // 최적화 정확도 설정
PointCloudBasedHandEyeCalibrator3D.SetScaleFactor(1.000000); // End Effector Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.SetResultScaleFactor(1.000000); // 결과 Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.Calibrate();

PointCloudBasedHandEyeCalibrator3D = CPointCloudBasedHandEyeCalibrator3D()

# 3D 매치 모델 설정
# Surface Match 3D 혹은 Vertex Match 3D의 모델 파일 경로 설정
# 확장자 : *.flsm, *.flvm
PointCloudBasedHandEyeCalibrator3D.Set3DMatchModel("")

##############################################
# CSV 파일로 포즈를 불러올 경우
PointCloudBasedHandEyeCalibrator3D.LoadEndEffectorPose("CSV File Path")
##############################################
# 직접 추가할 경우
listEndEffectorCoordinateTranslations = List[TPoint3[Double]]()
listEndEffectorCoordinateTranslations.Add(TPoint3[Double](0, 0, 0))
#		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateTranslation(listEndEffectorCoordinateTranslations)
listEndEffectorCoordinateAngles = List[TPoint3[Double]]()
listEndEffectorCoordinateAngles.Add(TPoint3[Double](0, 0, 0))
#		. . .
PointCloudBasedHandEyeCalibrator3D.SetCalibrationEndEffectorCoordinateAngle(listEndEffectorCoordinateAngles)
##############################################
 
PointCloudBasedHandEyeCalibrator3D.SetRotationType(ERotationType.EulerAngle) # End Effector 회전 타입 설정
PointCloudBasedHandEyeCalibrator3D.SetOptimizationMethod(EOptimizationMethod.Nonlinear) # 최적화 방법 설정
PointCloudBasedHandEyeCalibrator3D.SetEndEffectorAngleUnit(EAngleUnit.Degree) # End Effector 회전 각도 단위 설정
PointCloudBasedHandEyeCalibrator3D.SetCalibrationMode(CHandEyeCalibrator3D.ECalibrationMode.EyeInHand) # 캘리브레이션 모드 설정

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

PointCloudBasedHandEyeCalibrator3D.SetOptimalSolutionAccuracy(0.0000001) # 최적화 정확도 설정
PointCloudBasedHandEyeCalibrator3D.SetScaleFactor(1.000000) # End Effector Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.SetResultScaleFactor(1.000000) # 결과 Scale Factor 설정
PointCloudBasedHandEyeCalibrator3D.Calibrate()